Single-line laser radar and camera calibration method based on point, line and surface matching

Abstract

The invention belongs to the technical field of nuclear industrial robots, and aims to solve the problem of external parameter calibration of a monocular camera and a laser radar on large equipment. The invention discloses a single-line laser radar and camera calibration method based on point, line and plane matching, and the method comprises the steps: pasting a visual target on an intersection plane based on the multi-plane intersection features in a real environment, extracting the single-line laser point and line features, and the line and plane features based on the visual target in a monocular camera, and carrying out the calibration of the single-line laser radar and the camera. And then matching the extracted laser point and line segment features with a space plane and a straight line, constructing an error equation of the distance from the space point to the straight line and the distance from the line segment to the space plane, and finally carrying out laser radar and monocular camera external parameter optimization solution through a nonlinear optimization method. According to the application, the monocular camera and laser radar external parameter calibration on large equipment can be realized, and the calibration effect with higher precision and more stability can be obtained.

Description

Technical Field

[0001] The present application belongs to the field of nuclear industry robot technology, and in particular relates to a single-line laser radar and camera calibration method based on point, line and surface matching. Background Art

[0002] The ultimate heat sink during the operation of a nuclear power unit is the seawater cooling source, which is responsible for taking away the core preheating and cooling various nuclear safety equipment during shutdown. It plays an important role in the operation of nuclear power. The operating status of the water intake directly affects the safe operation and reliability of the power plant. The heat generated during the operation of the unit will cause marine organisms to attach to the cold source tunnel. The tunnel wall can be cleaned autonomously by a mobile carrier equipped with a robotic arm. To achieve the purpose of autonomous cleaning, it is necessary to perform multi-sensor fusion real-time positioning and perception of the carrier in the tunnel so that the mobile carrier and the robotic arm can operate safely. The underground tunnel environment cannot receive satellite signals for positioning, and the tunnel with a regular shape cannot support laser matching positioning. Most of the existing tunnel positioning technologies use wireless communication modules arranged in the tunnel and use carrier ranging technology to achieve positioning in the tunnel, but the cost of laying targets in advance is high. Considering the use of monocular cameras and single-line laser radars to achieve positioning perception of equipment in the water intake culvert pipeline, the first issue to be considered is the calibration of the technical safety of the monocular camera and the single-line laser radar.

[0003] The traditional method of monocular camera and single-line lidar calibration algorithm is generally to move the calibration plate in the common field of view of the lidar and camera, obtain the surface information of the calibration plate through the visual table on the calibration plate, extract the line information by extracting the ranging information of the lidar on the calibration plate, and then realize the calibration of the lidar and monocular camera through the constraints of the line in the plane. However, on large equipment, the camera and lidar are distributed, resulting in a very small common view range of the lidar and camera, or no common view area, which makes the traditional calibration method invalid. Summary of the invention

[0004] The purpose of this application is to provide a single-line laser radar and camera calibration method based on point, line and surface matching, which solves the problem of extrinsic parameter calibration of monocular camera and laser radar on large equipment.

[0005] In order to achieve the above objectives, this application provides the following technical solutions:

[0006] A single-line laser radar and camera calibration method based on point, line and surface matching, comprising:

[0007] Step 1: Extract point features and line features from single-line lidar data, extract targets and their corner points in the camera field of view, and then fit to generate planes and spatial lines;

[0008] Step 2: Associate the 2D laser point feature with the camera space straight line feature by distance, and select the 2D laser feature point with the closest distance to each space straight line and less than the threshold as the corresponding matching point; associate the 2D laser line segment feature with the 3D camera space plane by calculating the distance from the line segment to the plane and the angle between the line segment direction vector and the plane normal;

[0009] Step 3: Obtain the matching set of laser points and spatial lines and the matching set of laser line segments and spatial planes. Project the 2D laser points into the image space through the initial external parameters, construct the error equations of the distance from the spatial point to the line and the distance from the line segment to the spatial plane, and obtain the accurate external parameter transformation T between the single-line laser radar and the camera by minimizing the error equations. cl * .

[0010] In one embodiment, an ARUCO visual target is pasted on the intersecting plane, and the ID of the visual target, the position and pose of the target in the camera coordinate system, and the coordinates of the corner points of the target in the camera coordinate system are identified by a visual method.

[0011] In one embodiment, FALKO is used to extract point features in single-line laser radar data.

[0012] In one embodiment, a laser line segment feature extraction method based on seed region growing is used to extract line features in single-line laser radar data.

[0013] In one embodiment, step 1 includes:

[0014] Step 1.1, extracting ARUCO target and laser point line features;

[0015] Step 1.2: Generate a spatial straight line based on maker's plane fitting.

[0016] In one embodiment, the pose T of the marker in the camera coordinate system is cm for:

[0017]

[0018] In the formula, R cm represents the rotation matrix, t cm Represents the translation vector.

[0019] In one embodiment, the size of the marker is set to 2s, then the plane coordinates of its four corner points are They are:

[0020]

[0021] After getting the marker’s posture T cmAfterwards, the spatial coordinates of its four corner points are obtained according to the following formula:

[0022]

[0023] For a series of space points P c =[xyz] T , construct the following objective equation to solve the space plane, assuming that the parameters of the space plane are: π = [abcd] T , then:

[0024]

[0025] By solving the above equation, we get the space plane π;

[0026] Solve the straight line obtained by the intersection of the planes. In 3D space, the straight line The Plücker coordinates are expressed as:

[0027]

[0028] In the formula, is the direction vector of the line, It is a straight line The normal vector of the plane π formed by the camera optical center C;

[0029] Space straight line It can be calculated by the intersection of two planes π1 and π2. The formula is as follows:

[0030]

[0031] In the formula, For straight line The dual expression of [·] × Represents the antisymmetric matrix of a vector.

[0032] In one embodiment, the initial external parameters from a single-line laser to the camera are:

[0033]

[0034] The 2D laser feature points are:

[0035] p l =[xy 0] T

[0036] A straight line in 3D space is:

[0037]

[0038] Then the 2D point to the space line The distance calculation steps include:

[0039] Calculate the camera optical center to the space straight line The nearest point Q is as follows:

[0040] Q = [n] × d / d T d

[0041] In the formula, [n] × For straight line Antisymmetric matrix of direction vector;

[0042] After obtaining the coordinates of a point on the straight line in space, the initial external parameter matrix T between the laser radar and the camera is used cl The 2D laser feature point p l Project to the camera space coordinate system to get the space point P l ,as follows:

[0043] P l =R cl *p l +t cl

[0044] Then the 2D laser feature point to the space straight line The distance d l for:

[0045]

[0046] In the formula, n is a straight line Direction vector.

[0047] In one embodiment, the 2D line segment l and the 3D space plane π=[n P d] T Matching is performed by calculating the distance d from the line segment to the plane lp And the angle θ between the line segment direction vector and the plane normal is associated;

[0048] For each spatial plane, select the plane with an angle greater than the threshold θ th , the distance is the smallest and less than the threshold The line segments are taken as matching pairs;

[0049] Calculate the angle θ between the 2D line segment l and the plane π, denoted by p s , p e Respectively represent the starting point and end point of the line segment l, and obtain the 3D coordinate P of the line segment in the image space coordinate system s , P e , then the direction vector n of the line L It is expressed as:

[0050]

[0051] Then the normal vector n between the line and the plane π is P The angle θ between them is:

[0052]

[0053] Set point is the i-th point on the 2D laser line segment, then the distance d from the line segment to the plane π lp The calculation is as follows:

[0054]

[0055] Where N represents the number of laser points contained in the 2D line segment.

[0056] In one embodiment, the external parameter transformation T cl * for:

[0057]

[0058] In the formula, represents the i-th laser 2D feature point p i To the corresponding space straight line distance; represents the jth laser line segment l j to the corresponding space plane π j The distance; Σ p With Σ l Represent the weight factors of laser point and line segment errors respectively.

[0059] Compared with the prior art, the single-line laser radar and camera calibration method based on point, line and surface matching provided by this application has the following beneficial effects:

[0060] This application is based on the multi-plane intersection features in the real environment. Visual targets are pasted on the intersecting planes, and single-line laser points, line features and line and surface features based on visual targets in the monocular camera are extracted. The extracted laser points and line segment features are then matched with the spatial planes and straight lines, and error equations for the distance from the spatial point to the straight line and the distance from the line segment to the spatial plane are constructed. Finally, the external parameters of the laser radar and monocular camera are optimized and solved through nonlinear optimization methods.

[0061] Furthermore, when the overlapping area of ​​the field of view of the camera and the lidar is small or does not overlap, the present application can use this method to quickly achieve the technical safety calibration of the lidar and the monocular camera.

[0062] Furthermore, the present application can obtain a more accurate and stable calibration effect by matching 2D laser falko feature points with 3D spatial straight lines and matching 2D laser line segment features with 3D spatial planes. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] In order to more clearly illustrate the technical solution of the present application, the following is a brief introduction to the drawings required for the technical description.

[0064] Figure 1 Schematic diagram of multi-plane intersection features and pasted visual targets in a real environment provided for this application;

[0065] Figure 2 A flowchart of the single-line laser radar and camera calibration method based on point, line and surface matching provided in this application;

[0066] Figure 3 A schematic diagram of the implementation of the single-line laser radar and camera calibration method based on point, line and surface matching provided in this application;

[0067] Figure 4 A schematic diagram of the plane intersection provided for this application;

[0068] Figure 5 Point-line matching and line-surface matching effects provided for this application Figure 1 ;

[0069] Figure 6 Point-line matching and line-surface matching effects provided for this application Figure 2 . DETAILED DESCRIPTION

[0070] The following is further explained in detail through specific implementation methods.

[0071] like Figures 1 to 6 As shown, the present application provides a single-line laser radar and camera calibration method based on point, line, and surface matching, including:

[0072] Step 1: Feature extraction, including LiDAR point and line feature extraction and visual line and surface feature extraction.

[0073] Paste the ARUCO visual target on the intersecting plane, identify the ID of the visual target, the position of the target in the camera coordinate system and the coordinates of the target corner points in the camera coordinate system through visual methods, and use FALKO and the laser line segment feature extraction method based on seed region growth (existing technology) to extract point features and line features in the single-line laser radar data respectively;

[0074] Based on the coordinates of the target corner points in the camera coordinate system, a spatial plane is generated by fitting the spatial plane equation, and the Plücker coordinate representation of the intersecting line is solved by intersecting the planes.

[0075] Furthermore, step 1 specifically includes:

[0076] Step 1.1: ARUCO target and laser point and line feature extraction.

[0077] ArUco code (hereinafter referred to as marker) is a Hamming code generated by the ArUco library. Each marker has a black border for easy self-detection. The four corners of the border are the corner points of the marker, and the inside of the border is the binary code of the marker, where white is 1 and black is 0.

[0078] When the ArUco library detects a marker in an image, it can accurately identify its ID and give the pixel coordinates of each corner point in the original order, as well as the marker's posture T in the camera coordinate system. cm ,as follows:

[0079]

[0080] In the formula, R cm represents the rotation matrix, t cm Represents the translation vector.

[0081] FALKO and laser line segment feature extraction method based on seed region growing are used to extract point features and line features from single-line lidar data respectively.

[0082] Step 1.2: Plane fitting and spatial straight line generation based on maker.

[0083] Assuming the size of the marker is 2s, the plane coordinates of its four corner points are They are:

[0084]

[0085] After getting the marker’s posture T cm Afterwards, the spatial coordinates of its four corner points can be obtained according to the following formula:

[0086]

[0087] For a series of space points P c =[xyz] T , construct the following objective equation to solve the space plane, assuming that the parameters of the space plane are: π = [abcd] T , then:

[0088]

[0089] By solving the above formula, we can get the space plane π, and the result is as follows Figure 4 shown.

[0090] After obtaining the plane, we can solve the straight line obtained by the intersection of the planes. In 3D space, the straight line The Plücker coordinates are expressed as:

[0091]

[0092] In the formula, is the direction vector of the line, It is a straight line The normal vector of the plane π formed by the camera optical center C.

[0093] Space straight line It can be calculated by the intersection of two planes π1 and π2. The formula is as follows:

[0094]

[0095] In the formula, For straight line The dual expression of [·] × Represents the antisymmetric matrix of a vector.

[0096] Step 2: Match the laser point and line segment features with the spatial plane and straight line. The effect is as follows: Figure 5 and Figure 6 shown.

[0097] The matching of 2D laser feature points and camera space lines is associated by distance. For each space line, the 2D laser feature point with the closest distance to it and less than the threshold is selected as the corresponding matching point. First, the closest point Q from the camera optical center to the space line is calculated; then, through the initial external parameter matrix between the laser radar and the camera, the 2D laser feature point is projected to the camera space coordinate system to obtain the space point, and finally the distance from the 2D laser feature point to the space line is calculated.

[0098] The 2D laser line feature is associated with the 3D camera space plane by calculating the distance from the line segment to the plane and the angle between the line segment direction vector and the plane normal. First, the direction vector of the 2D line segment is calculated, then the angle between the line and the plane normal vector is solved, and finally, the distance from the line segment to the plane is solved based on the i points on the 2D laser line segment.

[0099] The matching between 2D laser feature points and camera space lines is based on distance association. For each space line, the closest distance to it and less than the threshold is selected. The 2D laser feature points are used as the corresponding matching points. Assume that the initial external parameters from the single-line laser (Laser) to the camera (camera) are 2D laser feature point p l =[xy 0] T And 3D space straight line Then the 2D point to the space line The distance calculation steps are as follows:

[0100] First, calculate the straight line from the camera optical center to the space The nearest point Q is as follows:

[0101] Q = [n] × d / d T d

[0102] In the formula, [n] × For straight line Antisymmetric matrix of the direction vector.

[0103] After obtaining the coordinates of a point on the straight line in space, the initial external parameter matrix T between the laser radar and the camera is used cl The 2D laser feature point p l Project to the camera space coordinate system to get the space point P l ,as follows:

[0104] P l =R cl *p l +t cl

[0105] Finally, 2D laser feature points to space straight lines The distance d l It can be expressed as:

[0106]

[0107] For a 2D line segment l and a 3D space plane π=[n P d] T The matching is done by calculating the distance d from the line segment to the plane. lp And the angle θ between the line segment direction vector and the plane normal is associated. For each spatial plane, select the plane with an angle greater than the threshold θ th , the distance is the smallest and less than the threshold The line segments are taken as matching pairs.

[0108] First, calculate the angle θ between the 2D line segment l and the plane π, denoted by p s , p e Represent the starting point and end point of the line segment l respectively, and the 3D coordinate P of the line segment in the image space coordinate system can be obtained. s , P e , then the direction vector n of the line L It can be expressed as:

[0109]

[0110] In getting the space straight line The direction vector n L Then, the normal vector n between the line and the plane πP The angle θ between them is:

[0111]

[0112] Set point is the i-th point on the 2D laser line segment, then the distance d from the line segment to the plane π lp It can be calculated as follows:

[0113]

[0114] Where N represents the number of laser points contained in the 2D line segment.

[0115] Step 3: Optimize the solution.

[0116] Through the above steps, the matching set of laser points and spatial lines can be obtained:

[0117]

[0118] And, the matching set of laser line segments and space planes:

[0119]

[0120] The 2D laser point is projected into the image space through the initial external parameters, and the error equations of the distance from the spatial point to the straight line and the distance from the line segment to the spatial plane are constructed.

[0121] After multiple observations, the error equation can be accumulated, and by minimizing the error equation, the accurate external parameter transformation T between the single-line laser radar and the camera is obtained. cl * .

[0122]

[0123] In the formula, represents the i-th laser 2D feature point p i To the corresponding space straight line distance; represents the jth laser line segment l j to the corresponding space plane π j The distance; Σ p With Σ l Represent the weight factors of laser point and line segment errors respectively.

[0124] In addition, in order to suppress outliers, the Huber kernel function is introduced as the weight function ω(·). The Huber kernel function is as follows:

[0125]

[0126] From the formula, we can see that when the absolute value of the error is greater than a certain threshold δ, the growth of the function changes from a square form to a linear form, which suppresses the rapid divergence of the overall error caused by mismatching. In addition, the Huber kernel function is smooth and can be differentiated at any value, which is convenient for optimization. The LM method integrated in the ceres optimization library is used for solution.

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

Claims

1. A single-line laser radar and camera calibration method based on point, line and surface matching, characterized in that: include: Step 1: Extract point features and line features from single-line lidar data, extract targets and their corner points in the camera field of view, and then fit to generate planes and spatial lines; Step 2: Associate the 2D laser point feature with the camera space straight line feature by distance, and select the 2D laser feature point with the closest distance to each space straight line and less than the threshold as the corresponding matching point; associate the 2D laser line segment feature with the 3D camera space plane by calculating the distance from the line segment to the plane and the angle between the line segment direction vector and the plane normal; Step 3: Obtain the matching set of laser points and camera space lines and the matching set of laser line segments and camera space planes. Project the 2D laser points into the image space through the initial external parameters, construct the error equations of the distance from the space point to the line and the distance from the line segment to the space plane, and obtain the external parameter transformation T between the single-line laser radar and the camera by minimizing the error equations. cl * .

2. The single-line laser radar and camera calibration method based on point, line and surface matching according to claim 1 is characterized in that: In step 1, the ARUCO visual target is pasted on the intersecting plane, and the ID of the visual target, the pose of the target in the camera coordinate system, and the coordinates of the target corner points in the camera coordinate system are identified by visual methods.

3. The single-line laser radar and camera calibration method based on point, line and surface matching according to claim 2 is characterized in that: FALKO is used to extract point features from single-line lidar data.

4. The single-line laser radar and camera calibration method based on point, line and surface matching according to claim 2 is characterized in that: The line features in single-line lidar data are extracted using a laser line segment feature extraction method based on seed region growing.

5. The single-line laser radar and camera calibration method based on point, line and surface matching according to claim 1 is characterized in that: Step 1 includes: Step 1.1, extracting ARUCO target and laser point line features; Step 1.2: Based on the plane fitting of maker, a spatial straight line is generated by plane intersection.

6. The single-line laser radar and camera calibration method based on point, line and surface matching according to claim 5 is characterized in that: In step 1.1, the marker's posture in the camera coordinate system is T cm for: In the formula, R cm represents the rotation matrix, t cm Represents the translation vector.

7. The single-line laser radar and camera calibration method based on point, line and surface matching according to claim 5 is characterized in that: In step 1.2, the size of the marker is set to 2s, then the plane coordinates of its four corner points are They are: After getting the marker’s posture T cm Afterwards, the spatial coordinates of its four corner points are obtained according to the following formula: For a series of space points P c =[xyz] T , construct the following objective equation to solve the space plane, assuming that the parameters of the space plane are: π = [abcd] T , then: By solving the above equation, we get the space plane π; Solve the straight line obtained by the intersection of the planes. In 3D space, the straight line The Plücker coordinates are expressed as: In the formula, is the direction vector of the line, It is a straight line The normal vector of the plane π formed by the camera optical center C; Space straight line It can be calculated by the intersection of two planes π1 and π2. The formula is as follows: In the formula, For straight line The dual expression of [·] × Represents the antisymmetric matrix of a vector.

8. The single-line laser radar and camera calibration method based on point, line and surface matching according to claim 1 is characterized in that: In step 2, the initial external parameters from the single-line laser to the camera are: The 2D laser feature points are: p l =[x y 0] T A straight line in 3D space is: Then the 2D point to the space line The distance calculation steps include: Calculate the camera optical center to the space straight line The nearest point Q is as follows: Q=[n] × d / d T d In the formula, [n] × For straight line Antisymmetric matrix of direction vector; After obtaining the coordinates of a point on the straight line in space, the initial external parameter matrix T between the laser radar and the camera is used cl The 2D laser feature point p l Project to the camera space coordinate system to get the space point P l ,as follows: P l =R cl *p l +t cl Then the 2D laser feature point to the space straight line The distance d l for: In the formula, n is a straight line Direction vector.

9. The single-line laser radar and camera calibration method based on point, line and surface matching according to claim 1 is characterized in that: In step 2, the 2D line segment l and the 3D space plane π = [n P d] T Matching is performed by calculating the distance d from the line segment to the plane lp And the angle θ between the line segment direction vector and the plane normal is associated; For each spatial plane, select the plane with an angle greater than the threshold θ th , the distance is the smallest and less than the threshold The line segments are taken as matching pairs; Calculate the angle θ between the 2D line segment l and the plane π, denoted by p s , p e Respectively represent the starting point and end point of the line segment l, and obtain the 3D coordinate P of the line segment in the image space coordinate system s , P e , then the direction vector n of the line L It is expressed as: Then the normal vector n between the line and the plane π is P The angle θ between them is: Set point is the i-th point on the 2D laser line segment, then the distance d from the line segment to the plane π lp The calculation is as follows: Where N represents the number of laser points contained in the 2D line segment.

10. The single-line laser radar and camera calibration method based on point, line and surface matching according to claim 1, characterized in that: In step 3, the external parameter transformation T cl * for: In the formula, represents the i-th laser 2D feature point p i To the corresponding space straight line distance; represents the jth laser line segment l j to the corresponding space plane π j The distance; Σ p With Σ l Represent the weight factors of laser point and line segment errors respectively.

Images