Important additions in February 2023
I personally felt that this code was not easy to use and had too many pitfalls, so I later changed it to another one. It is recommended that everyone use the new code.
For details, please see an updated blog summary: [Learning Summary] Lidar and camera external parameter calibration: code (cam_lidar_calibration)
Over the past week, I have learned and debugged the codes for lidar and camera external parameter calibration, and stepped on a lot of pitfalls, which I will record here.
0. References:
Code source: https://github.com/ankitdhall/lidar_camera_calibration
Reference paper: LiDAR-Camera Calibration using 3D-3D Point correspondences
Modified code: https://github.com/LarryDong/lidar_camera_calibration
I found several open source calibration codes before, and this code has the most stars on github, which I think is more reliable. As a result, this code is really hard to describe: the logic is confusing, there are so many details that need to be paid attention to, and there are many pitfalls. This week or so has been just digging around and debugging. Today I was finally able to run it normally and produce a correct (but not accurate) result, which I will record here.
Reasons for confusing code:
- A large number of variable definitions are not used, and the distance between the definition and actual use is too misleading;
- Multiple parameter files need to be modified, and parameter management is confusing; parameters are read from different files, and some temporary variables are also stored in files and then read and called in different functions, as well as the use of global variables;
- Some details are not emphasized in the official usage documentation.
1. Basic principles
The basic principle of calculating the external parameters of radar and camera is relatively simple: you only need to find the coordinate relationship between the point under the radar system and the corresponding point under the camera system, and then calculate the external parameters. A previous blog summarized several commonly used methods: [Learning Record] Lidar and camera calibration
The core of the problem is, how to determine the correspondence relationship, that is, correspondence?
The basic principle of the code used: use a rectangular piece of cardboard, use lidar to detect the boundary line of the cardboard, then determine the corner points of the cardboard, and obtain the coordinates of the corner points of the cardboard in the lidar system; the corner points of the camera itself cannot provide depth information. But we can use aruco to provide the corner points of the marker with depth, and then calculate the coordinates of the four corners of the cardboard.
2. Code Overview
First, you need to install it according to the wiki in the original code link, and then you need to modify some parameter configuration files. If you're lucky and figure out how to use it, you might be able to get results right away. However, many details are not explained clearly in the original code readme, so this blog will organize them in detail.
The logic of the entire code is as follows:
First, the core of this code is the lidar_camera_calibrationinitiated find_transform.launchcalibration program. The data received by this program includes:
- Lidar raw scan data
- aruco marker pose under the camera
- Corresponding parameter configuration file
The lidar data is provided directly by the lidar startup node, the aruco marker message aruco_mappingis provided by the node, and the corresponding parameter configuration file is provided by "lidar_camera_calibration/conf/config_file.txt".
After receiving the above data, the core code detects the boundary lines and corner points of the cardboard on the radar data, and finally aruco_mappingperforms iterative solution with the provided pose. The procedures and nodes are introduced below in order.
3. Detailed explanation of the code
3.1 Data flow
First look at it from the perspective of data flow.
Image data flow: camera driver-> aruco_mapping->"lidar_camera_calibration/conf/transform.txt" file-> find_transformnode. The camera driver transmits the complete image to aruco_mapping, aruco_mappingobtains the camera's internal parameters (including resolution, K, distortion parameters, and projection parameters, etc.) from its configuration file "aruco_mapping/data/xxx.ini" file, and also obtains the camera's internal parameters (including resolution, K, distortion parameters, and projection parameters, etc.) from "aruco_mapping/launch/ Get the marker parameters from the aruco_mapping.launch" file. Finally, aruco_mappingthe pose data is saved in the "lidar_camera_calibration/conf/transform.txt" file, and then read by the node, and then used find_transformaccording to the parameters of the marker and board (hereinafter referred to as "board") obtained from "lidar_camera_calibration/config_file.txt".
The parameters are calculated under the camera at the four corners of the cardboard.
LiDAR data flow: LiDAR driver -> find_transform, relatively simple. The lidar sends the original point cloud data directly to find_transformthe node. The node projects the boundary points in the point cloud to the camera image based on the scanning situation and the configuration in "lidar_camera_calibration/conf/config_file.txt" for visualization and manual annotation. .
There is also some intermediate data, such as find_transformthe node temporarily storing the board corners detected by lidar and the calculated 3D coordinates of the corner points in the camera system in "lidar_camera_calibration/conf/points.txt" for subsequent use. of reading.
3.2 aruco_mappingnodes
The above data flow has explained that aruco_mappingthe node is responsible for obtaining the original image and outputting the coordinates of the marker in the image under the camera system. The image topic name and marker size it accepts aruco_mapping.launchare given in, and the camera parameters are also required. The camera parameters need to be calibrated in advance and the "/data/xxx.ini" configuration file needs to be modified according to the specified format.
Output the 6DoF pose to the "lidar_camera_calibration/conf/transform.txt" file. This part of the code is relatively complete. For details, please refer to the aruco_mapping official documentation .
Key points : It is necessary to modify the receiving topic name, marker parameter file, and camera parameter .ini file.
3.3 Lidar node
The lidar node is relatively simple, that is, the lidar is driven under ros, and the lidar data can be released after startup. Received later find_transform.
3.4 find_transformnodes (core)
This node is the core of the algorithm and will be expanded in order.
3.4.1 Message reception and callback function
Starting from the main function, the first session first receives the topic sent by the above-mentioned nodes. One of the choices is whether to use "callback" or "callback_noCam". The difference between the two is: whether the camera's internal parameters are received from the topic or read from the file. Essentially the same, we use reading from the file, so that there is no need for aruco_mappingnodes or camera driver nodes to send topic data. The file read is still the "conf/config_file.txt" mentioned above.
Here, the message synchronization mechanism under ROS is used during the callback message_filtersto ensure that the camera data and the lidar data are synchronized. But since the scene can be considered static, this synchronized start doesn't make much sense.
3.4.2 Point cloud data reception
Entering the callback function, the first thing is to receive the data sent by the lidar driver node. Here I encountered the first pitfall: [fromRosMsg() reported an error Failed to find match for field “intensity”] . The reason for this is because the intensity field of the Leishen lidar I used is defined as a strange uint type, and the intensity field under RosMsg is float, so it cannot be converted directly. To do this, you need to redefine the custom point data type and copy function. See the learning summary above for details. (In fact, intensity is useless)
After the point cloud data is received, it needs to be converted into a unified point cloud format. The author uses a customized one PointXYZRID. The useful fields are "I" for intensity and "R" for ring, that is, which line of data is scanned. Generally, radars provide scanned line beam information, which can be added during radar conversion. If the wire harness information is not provided, the wire harness can be determined by calculating the pitch angle similar to that in the A-LOAM source code.
Then we encountered the second pitfall: the problem of invalid data points, namely NaN. Since intensity needs to be used for boundary point detection later, and if the lidar emits NaN data, errors will occur during subsequent normalization, so NaN data needs to be proposed. Unfortunately, PCL's own NaN data removal function removeNaNFromPointClouddoes not support the custom "PointXYZRID" data type, so it cannot be called directly. I can only handwrite one to determine whether each point is NaN.
3.4.3 Point cloud transformation
transformAfter obtaining the point cloud, first roughly transform the point cloud in the Lidar system to the camera coordinate system through a function. This step is so that the subsequent point cloud can have a rough position in the camera, which is convenient for the camera to draw and display. At this point, the approximate transformation parameters are estimated by us, using the initial rotation and translation given under "conf/config_file.txt". The translation and rotation here are "the camera system rotates to the radar system through this translation", that is, "the coordinate points under the radar system become the coordinates under the camera system through rotation and translation." The three angles in line 12 of the parameter file are in the form of ZYX Euler angles, eularAngle(2,1,0), and the unit is radians. You can use the calculated rotation matrix to determine whether the given rotation angle is correct.
I would like to add here that we must pay attention to the definition of the coordinate system of the lidar and the coordinate system of the camera. The coordinate system of the camera here is defined as: the z-axis is outward along the lens, the y-axis is downward, and the x-axis is defined by the right-hand system.
3.4.4 Point cloud boundary point detection
Boundary point detection is intensityByRangeDiff()provided by the function to obtain all boundary points in the scene. This function determines the boundary point by judging the intensity of change within a certain range of the same ring that matches the intensity. However, the intensity here is specifically the range used, that is, the distance. details as follows:
Starting from the second point of each circle, the difference between the range of the previous and next points is judged as the intensity. Then call normalizeIntensitythe function for normalization (Note: All distances are required to be valid here. If extreme distances are measured, such as very far points or NaN points, the normalization effect will not be good). After normalization, points whose intensity is greater than a certain threshold and within the range given by xyz are retained. The threshold and the range of xyz are given from lines 2-5 of "conf/config_file.txt". Let me make a complaint here. I spent a long time converting intensity before, but I didn’t use it at all later. In addition, it is clearly detected based on range, but it is not called intensity here, which caused a lot of ambiguity in understanding. It may be for the convenience of using the PointXYZI format.
After extracting the boundaries, return all boundary points in the scene. The next step is to detect the corner points.
3.4.5 Cardboard corner detection in point cloud
Corner detection is getCorners()a function. Let’s complain about the internals of this function. The opencv code is poorly written, with various masks and back-and-forth assignments of unknown meaning.
The first step is to project the boundary points in the entire scene into the camera image, projectand the function is, "which lidar boundary points can be seen by the camera image". The camera's projection matrix is used here, from the config parameters read from lines 8-10 of "conf/config_file.txt".
The second step is to manually mark the boundary points of the cardboard. Since there are many boundary points in the scene, you need to know which are the boundary points of which side of the cardboard. Therefore, manual annotation is used. cloudAfter running to this step, a window and a window will appear polygon, with the former showing the boundary points seen from the image perspective. We cloudclick 4 points sequentially in the window to form a polygon that frames a boundary of the cardboard. After each click, press any key to store the coordinates. After clicking 4 points, polygonthe window will draw the drawn polygon and the boundary points framed by this polygon. This completes the selection of the first boundary, and then selects the other three boundaries in order.
Figure: The left side shows the boundary point detection, and the right side shows the first boundary selected by the frame.
Note 1 : If the boundary points without lidar scanning are not projected into the camera as expected, please check whether the initially given camera and radar estimated external parameters (mainly rotation) are reasonable.
Note 2 : When selecting an edge here, form a polygon in clockwise or counterclockwise order. It cannot be worse, otherwise there will be problems when using opencv to determine whether a certain point falls into the polygon; Note 3: When selecting 4 edges, follow
the steps Clockwise order, this order should be aruco_mappingconsistent with the rotation of the detected marker. If we follow the placement of markers in the picture given by the official code, that is, the x-axis is toward the "upper left" and the z-axis is "upper right", as shown in the figure below, then when marking the boundary of the radar point, first mark the upper left edge, and then mark The upper right side. Because the four corner points are required to correspond in order, and when the code subsequently searches for the corner points of the radar, the order stored is the intersection point of the two marked boundaries, and the order of the lower corner points of the camera system in the subsequent function is upper right - lower right find_transformation()- Bottom left - top left order.
In the third step, note that there is an iteration here. This iteration is the outermost callback_noCam()iteration, that is, iterations 3.4.1~3.4.6, rather than iteration in this function. The number of iterations is MAX_ITERATION times, and the default is 100. During the iteration, we marked the four polygons of the four boundaries for the first time, and then automatically extracted the boundary points of these four polygons and the boundary points of the marker for calculation.
The fourth step is to calculate the corners of the cardboard. Since each boundary was extracted in the previous step, to calculate the 3-dimensional straight line parameters of each boundary, PCL's SampleConsensusModelLine method is used for straight line fitting, and then PCL is used to calculate the mid-perpendicular line of lineToLineSegmenttwo adjacent straight lines, and take the midline points as corner points.
At this point, the extraction of cardboard corner points in one iteration is completed. The fifth step is to store the extracted 3D corner points in the temporary file "/conf/points.txt". If there are n markers, n*4 rows of corner points will be stored.
3.4.6 Detection of cardboard corners and calculation of external parameters in the camera
Cardboard corner detection and external parameter calculation under the camera system are both in the function find_transformation(). This function needs to receive aruco_mappingthe marker message sent by the node lidar_camera_calibration_rt, and read the lidar lower corner points from the above-mentioned "/conf/points.txt" file for registration.
The first step is to calculate the three-dimensional coordinates of the corner point in the camera. Directly read the parameters of aruco, and then calculate the corner points based on the parameters and the dimensions of the board and marker (defined by the next few lines of data in "conf/marker_coordinates.txt"). From here we can determine how many corner points are under the camera. coordinate of. The calculation method is very simple. Since the marker defines the xz plane, the y coordinates of the four corners of the cardboard on the same plane are all 0. The xz coordinates can be determined by the margin of the markder and the size of the cardboard. After confirmation, all point data are stored in the temporary file "/conf/points.txt" in a write-through manner. Let’s make a complaint, it’s really a nonsense.
In the second step, readArray()the function reads the three-dimensional coordinates of all corner points in "/conf/points.txt" under lidar and camera.
The third step is calc_RT()to iteratively calculate the external parameters by the function. The calculation of this function is a typical ICP algorithm. First, the translation is calculated, and then the SVD decomposition is used to calculate R. The Rt here is the Rt when the camera system changes to the lidar system, that is, the coordinates in the lidar system are transferred to the camera system.
As mentioned in 3.4.5, a total of 100 iterations were carried out. Each iteration went through the functions of 3.4.6 to calculate the Rt from the camera system to the lidar system. Then the iteration ended after 100 times and the average was calculated. The Rt. It should be noted here that the program outputs several Rt and T. The calculated average R is only the increment under the initial given rotation, not the actual R. The upper left corner 3x3 matrix contained in the complete T is is the real external parameter R, and 3x1 in the upper right corner is the external parameter t. Note that the 100 iterations here use global variables, which is the global variable used when the polygon was marked for the first time in 3.4.2 iteration_counter. Here again, global variables are used to pass parameters.
Final result: The red box is the real Rt
The lidar is located at 5cm in the y-axis direction of the camera. It is basically considered that the calibration is correct, but it is not accurate yet. Will be improved later.