Abstract
Heterogeneous sensors fusion of a camera and a laser-rangefinder can greatly improve the environment perception ability, and its primary problem is the calibration of depth scan and image information. At first, the mapping relationship among world coordinate system, camera coordinate system, and image plane is discussed, and then the calibration of camera intrinsic parameters is achieved. Moreover, the intrinsic and extrinsic parameters separated calibration is presented for a camera and a laser-rangefinder, and the characteristic identification is adopted by two intersection calibration boards with a certain angel for fusion characters extraction. Furthermore, the particle swarm optimization is proposed for the extrinsic parameters estimation with different objectives, and the Gaussian elimination is utilized for the initial particle swarm. The simulation and real experimental results show that the standard deviation of calibration error in the 21-group experiments is decreased by 10.175%, and it also proves the accuracy and effectiveness of our approaches.
1. Introduction
The ability of environmental perception has been considered as an important functionality for heterogeneous multisensor system [1]; heterogeneous sensors system usually includes camera and laser range finders, which are applied to compensate for their drawbacks of each sensor in order to be more reliable. Among those sensors, a camera has been the most popular sensor for recognizing objects, but the camera is too sensitive to the light and the weather and has some limits to acquire depth information, while the laser range finders can give more accurate depth information [2]. Meanwhile, they are also used as the main sensor for autonomous navigation [3]. Therefore, a laser- rangefinder and a camera have different abilities to capture the surrounding information, if these abilities of the two sensors are combined to improve the environment perception ability for multisensor system. However, one of the major problems for heterogeneous sensors fusion is to match the data from these different sensors [4]. The information fusion of two sensors requires knowing in advance the relative pose between the camera and the laser-rangefinder. Thus our paper addresses the intrinsic parameters and extrinsic parameters separated calibration, and then the extrinsic parameters are estimated by the particle swarm optimization (PSO) to decrease the calibration error.
The number of published works on the intrinsic and extrinsic parameters separated calibration of a camera and laser-rangefinder is relatively little. Meanwhile, the particle swarm algorithm that is adopted to optimize the extrinsic parameters in this paper is also novel. As we all knew that the most classic calibration method was proposed by Zhang and Pless [5], it described a practical procedure where a check board pattern was freely moved in front of the two sensors; it was one of the successful and valid algorithms, so we utilized a part of Zhang’s algorithm for intrinsic parameters calibration in this paper. As for the extrinsic parameters calibration, it was achieved by freely moving a check board pattern in order to obtain plane poses in camera coordinates and depth information in the LRF reference frame [6]. Meanwhile, an external parameter calibration method for multiple cameras with nonoverlapping fields of view using a laser-rangefinder (LRF) was presented in [7]. And a modular approach had been extensively tested during VIAC which had offered a unique chance to face pros and cons of different calibration procedures in [8]. Furthermore, the original nonlinear calibration model of multiple LIDARs was reformulated as a second-order cone program (SOCP) on a mobile vehicle platform in [9], the nonlinear distortion of the camera was considered, and the calibration parameters were determined with the least square error in [10]. However, the intrinsic parameters and extrinsic parameters mixed as a kind of parameters matrix to calibrate at the same time, which led the space of solution to become larger; meanwhile, the errors of parameters estimation are increased. Therefore, the camera’s intrinsic parameters and extrinsic parameters were separated in order to improve the accuracy of calibration [11]; nonlinear least square and nonlinear Gauss-Newton method are utilized to optimize parameters. However, the performance of parameters optimization is limited by the algorithm. Furthermore, a novel laser-rangefinder calibration method was proposed by using genetic algorithm to overcome the problem [12] that the conventional camera calibration methods cannot correct the misalignment of this rangefinder. The fitness function estimated the difference between the actual image outputs and the calculated image outputs. Therefore, we proposed particle swarm optimization (PSO) to optimize the extrinsic parameters for heterogeneous sensors calibration of laser-rangefinder and camera in this paper; at the same time, the Gaussian elimination is utilized to initialize the particle swarm, and we improve the fitness function according to decreasing the calibration error. Therefore, not only is it effective in calibrating the separated parameters based on PSO, but also it can decrease the error of calibration.
The rest of paper is organized as follows. Section 2 describes the coordinate transformations from laser-rangefinder to optical image plane. In Section 3, we introduce how to calibrate the intrinsic parameters and the extrinsic parameters separated calibration method. Then the extrinsic parameters estimation and optimization based on PSO are designed in Section 4. Section 5 exhibits some comparison experimental analysis of the laser-rangefinder and the camera fusion calibration based on PSO. At last, Section 6 provides the conclusion for this paper.
2. Coordinate Transformations from Laser-Rangefinder to Optical Image Plane
The hardware of information fusion platform mainly consists of a laser-rangefinder and a camera. Here the type of laser-rangefinder is SICK LMS291 [13], which is noncontact measurement systems, and scans their surroundings two-dimensionally. We select horizontal angle field of 180°, the interval angle is 1°, and transmission rate is set as 500 Kbps. The data structure of laser-rangefinder is a kind of 1 × 181 dimensional matrix array, so a median filter is utilized to remove the noise data. Moreover, the camera is FFMV-03MTC-CS, using 1394 transmission mode and its resolution of , because the camera of IEEE 1394 bus can be satisfied for the demand of real time.
The observed objects captured by laser-rangefinder are distance information of a depth plane in the space, while the objects collected by camera are optical information according to the optical principle. Due to the heterogeneity of data acquisition, the images of camera and the datum of laser- rangefinder are heterogeneous. Hence, it is significant to integrate those heterogeneous data into the same coordinate system for information fusion. Therefore, there are two main steps to map laser- rangefinder information to optical images plane through coordinate transformation [14]. Firstly, we need to obtain a relatively accurate transformation matrix according to ideal physical model, which can ensure data captured by laser-rangefinder mapping into optical image coordinate system. Secondly, since image coordinates of each pixel are discrete, it is necessary to utilize gray-scale interpolation method for each coordinate transformation after spatial coordinate mapping, which can make them fall on the exact pixel points even when the coordinates of four surrounding points are not integer.
2.1. Spatial Coordinate Transformation
Here laser-rangefinder coordinates system is , is the coordinates of the optical image plane, and the target object coordinates in the world coordinate system are . The most widely used model of camera is the typical pinhole model of camera [15]. The equation of the model is where represent coordinates of the target point in the image plane and are the world coordinates of object; here, they are also considered as a point in camera coordinate system. are the scale factors along the axes of pixel coordinates, which are defined as adjacent pixels physical distance ratios of horizontal and vertical individually in the images. are the coordinates of image center in pixels, and means focal length. The orthonormal rotation matrix and translation vector are combined for transforming from laser-rangefinder depth information to the world (or camera) coordinate; we obtain conversion expression (2) according to coordinate transformation rules: So the laser-depth coordinates are transformed to the optical image coordinates and we gain According to (3), the world coordinates of the object are denoted by , so the points of laser-rangefinder mapping to optical images are completed successfully.
2.2. Gray-Scale Interpolation for Information Fusion
Since the coordinates of each pixel are discrete, it may fall on the noninteger pixel after coordinate transformation. The interpolation method is needed to use after each coordinate transformation in order to fall on the exact pixel points. Hence, we use neighbor interpolation method to realize gray- scale interpolation, as is depicted in Figure 1. Here, if the laser-rangefinder transformed coordinates are not integers, the four-pixel coordinates that are surrounding the laser-rangefinder mapping point after spatial coordinate transformations are captured firstly. Secondly, the distance between the lasers mapping point and these four surrounding pixels is computed. Thirdly, the original laser-rangefinder point coordinate is substituted by the minimum distance mapping pixel point coordinate.
2.3. The Calibration Parameters Analysis
The camera parameters are classified into the intrinsic parameters and extrinsic parameters. Generally, the inherent characteristics and properties of the camera are determined by its intrinsic parameters, since they are not going to change for the same camera. In other words, for a camera, if the focal length or the mechanical structure keeps invariability, its intrinsic parameters are fixed. However, the extrinsic parameters represent the pose and orientation information of the camera in the world coordinate system. Therefore, the extrinsic parameters can be denoted by the orthonormal rotation matrix and translation vector. Wherefore, it is necessary to measure intrinsic parameters and extrinsic parameters of the camera separately; especially when position and orientation of the target objects are restored from the optical image to spatial coordinate, the process is called calibration for a camera and a laser-rangefinder. Thus intrinsic and extrinsic parameters of the camera are indispensable for data fusion [16].
From Figure 2, are the coordinates of on the scene plane in the world system, the line between and the camera interacts with the optical plane on , it is an image ideal point, and its coordinates are called . Meanwhile are the coordinates of an actual image point mapping in camera image plane. Rotation matrix and displacement translation vector are used to describe the coordinate transformation [17] as Here, is called camera extrinsic parameters matrix, which is determined by the pose and orientation of the camera in the world coordinate system. Due to the distortion of the optical image, the change of focal length, and optical path’s centerline, and are not coincided. Consequently, it is necessary to define a transformation matrix to indicate the relative position between them. Assuming that are the distances of two adjacent pixels of the optical image in -axis direction and -axis direction and are the coordinates on the optical image, are the intersection coordinates of optical path’s centerline on the optical image plane, setting each pixel as a unit. According to the principle of pinhole imaging, we obtain Further, we find the transformation relationship of the camera coordinate system mapping to the optical image coordinate system according to where represents the camera’s intrinsic parameters matrix, which is determined by its inherent characteristics. Consequently, the relationship between the world coordinate system and the optical image coordinate system can be expressed as (7) according to (4) and (6): Here, is the camera extrinsic parameters matrix, is the camera’s intrinsic parameters matrix, are the coordinates on the optical image, are the coordinates of , and is the coordinate of as shown in (4).
3. The Calibration and Analysis of Camera Intrinsic Parameters
The camera calibration algorithm [5] is utilized to estimate its intrinsic parameters. Therefore, the intrinsic parameters are substituted into the next spatial coordinate transformation as known parameters for heterogeneous data fusion. In this paper, Zhang’s algorithm is utilized for camera’s intrinsic parameters calibration partly. Meanwhile, the “Camera Calibration Toolbox for Matlab” of Jean-Yves Bouguet is adopted [18]. Firstly, planar checkerboard is utilized as the camera calibration board, and its grid side is 30 mm. Secondly, multiple angles calibration images are collected; thus it is unnecessary to fix positions and orientations for the camera’s intrinsic parameters calibration. Thirdly, in a clockwise direction, starting from the top left corner, initial values of each corner point are set through the ratio of selection box, as is shown in Figure 3; additionally, the side length of each small grid should be given before the calculation (e.g., 30 mm). The system supposes the corner search range as five pixels. Fourthly, inputting corner point information into Zhang’s parameters calibration tools, we can acquire intrinsic parameters of the camera. Meanwhile, Camera Calibration Toolbox for Matlab [18] is utilized; it can also output extrinsic parameters of the camera in the three-dimensional coordinates. After calculation, we obtain intrinsic parameters as
4. Extrinsic Parameters Separated Calibration Estimation Method
The calibration of a camera and a laser-rangefinder is also considered as an optimization problem, which is regarded to minimize the distance between the features from camera measured objects and their actual position. After calibration, as any points of the world coordinate system, we can connect it with the optical center by a straight line, and then this line will intersect with the optical plane. Therefore its precise coordinates on the optical image can be located by this intersection point. The above is significant and crucial for data fusion of the laser-rangefinder and the camera, which affects the fusion precision and efficiency closely. Here, the coordinate transform formula (9) is gained according to (3):
Here, laser-rangefinder data are denoted in polar coordinates. As for the fusion platform, a camera is fixed to the top of the laser-rangefinder; the Cartesian coordinates of laser-rangefinder points are described as where is the measure distance of laser-rangefinder and is the scan angle of laser-rangefinder. Then the rotation matrix is represented by the combination of rotation amounts along -axis direction and -axis direction, so if we define the coordinate transform formula equation (9) as the form of
In (11), , , , , , , , and , so the parameters to be determined of are nonlinear. Here are the discrete coordinates of laser-rangefinder; are the optical image coordinates. Meanwhile, assuming each point belonging to the line separating of the green plank and the white plank (shown in Figure 4), which satisfies the characteristic linear equation of (12), it is also the intersection line of two discriminating planes: where , are characteristic line parameters; the intrinsic and extrinsic parameters separated estimation is proposed and the separated estimation equation of the camera’s intrinsic and extrinsic parameters is gained as (13) according to (9) and (12):
According to (13), the separated calibration of intrinsic and extrinsic parameters are required to solve these 13 unknown parameters (, , , , , , , , , , , , ). By above analysis, we can gain according to (10) here, , and are the intrinsic parameters, which are gained from (8). So only , and extrinsic parameters are left to be estimated.
4.1. The Characteristic Parameters Identification
Additionally, are also seen as the known parameters in (13), because they can be calculated by characteristic line and characteristic points on the separating intersection line as (12). Here, we identify those characteristic parameters through any two characteristic points, which are extracted on the characteristic line of , so and characteristic parameters can be determined. Meanwhile, as for laser-rangefinder data, are considered as the maximum curvature points in the intersection line of scanning plane and calibration plate, which need to fall in the line . Therefore, the maximum curvature object point is extracted from a series of points on the intersection with calibration plate; then are gained.
Moreover, lots of experimental data are needed to substitute into (13) to solve and estimate the other parameters. These various effective experimental data are obtained by altering relative pose between the objects and camera under different experimental surroundings, for instance, changing the inclination of the object or adjusting the distance between the object and the camera. Significantly, it has to be guaranteed that laser-rangefinder and optical image data are collected synchronously.
4.2. Extrinsic Parameters Separated Calibration Based on Particle Swarm Optimization
In fact, , and extrinsic parameters can be estimated by 9 equations from (13) which are designed by 9 different scene experiments after the , and and , and are all solved. As we all know that Gaussian elimination (also known as row reduction) is an algorithm for solving systems of those linear equations, in this paper, Gaussian elimination is utilized to choose proper initial solution for the particle swarm. However, the initial solution is not the best solution to estimate the extrinsic parameters which may bring into lots of calibration error. Therefore, it is beneficial for the particle swarm optimization algorithm to improve extrinsic parameters estimation performance.
Furthermore, the extrinsic parameters estimation is also considered as the parameters optimization process; we can infer (14) from (13); that is to say, some measurement noises are added into the coefficient matrix, which may also satisfy the transform of (13); a random noise is added into (13); it satisfies nonzero solutions as the following (14); if tends to be zero infinitely, thus (14) is similar equivalent to (13): Therefore, the calibration of a camera and a laser-rangefinder is also considered as an optimization problem. In addition, there are some objectives for the extrinsic parameters calibration; the major objective is to minimize the distance between the features of camera measured objects and their actual position. Therefore, the major objective of extrinsic parameters calibration is divided into two optimization subobjectives; one minimized the sum of squared error as is shown in
The other minimized the sum of distances from the points to the lines of ; (16) is the other objective: Here,
In this paper, the particle swarm optimization is proposed for the extrinsic parameters calibration, so we suppose that the search space is D-dimensional; here , that means there are 9 extrinsic parameters to be estimated; then the th particle of the swarm can be represented by a -dimensional vector . The velocity of the particle can be represented by another -dimensional vector . The best previously visited position of the th particle is denoted by , defining as the index of the best particle in the swarm. The swarm is manipulated according to the following: where , is the size of the swarm, is called inertia weight, and are two positive constants, called cognitive and social parameter, respectively, assuming , and are all random generator number between 0 and 1. Two variants of the PSO algorithm were developed, one with a global neighborhood and one with a local neighborhood. According to the global variant, each particle moves towards its best previous position and towards the best particle in the whole swarm. On the other hand, according to the local variant, each particle also moves towards its best previous position and towards the best particle in its restricted neighborhood. Meanwhile, we set the evaluation function , and we utilize the Gaussian elimination method to solve the extrinsic parameters and initialize particle swarm initialization; some details are shown in Algorithm 1, and the experiments are illustrated in Section 5.3.
| ||||||||||||||||||
5. Data Fusion Experiment Analysis for the Calibration of Laser- Rangefinder and Camera
5.1. Extraction Characteristic Parameters of
According to the characteristic line of camera images, which is the intersection line of two calibration boards shown in Figure 4, the characteristic line is able to be extracted through selecting any two points manually, which are not the same points on the image, and those two points can determine the characteristic line; then the characteristic parameters of can be calculated.
5.2. Feature Point Extraction Based on Laser-Rangefinder
Before extracting feature point , the observational data in the intersection line of laser- rangefinder scanning plane and calibration plate are needed to collect firstly. Moreover, the laser-rangefinder data of calibration board should be shown as “arrow” pattern, because the pattern can be set manually. Furthermore, the two edges straight lines of the arrow-shaped pattern are extracted; then the intersection of the two straight lines can be calculated to obtain . As is shown in Figure 5, the two-line intersection is the object feature point .
(a) Feature extraction
(b) Feature points extraction for one line
(c) Feature points extraction for the other lines
5.3. PSO for the Extrinsic Parameters Separated Calibration and Optimization
Generally, extrinsic parameters can be calibrated by 9-group different scene experiments. Here we design 21-group different scene fusion experiments to collect data for the extrinsic parameters calibration and optimization; thus there are kinds of equations to solve the extrinsic parameters by Gaussian elimination for the particle swarm initialization, and here we select randomly 13 kinds of solutions to initialize the particle swarm; that is to say, the number of particles is set as 13, and here the threshold value of evaluation function is 0.5, and we hope that the extrinsic parameters not only should fit for a special experiment but also should adapt to a majority of fusion experiments. Therefore, we set the standard deviation of evaluation function for the 21-group fusion experiments as the fitness function to optimize the extrinsic parameters, as is shown in Here , , is the th scene fusion experiment for the sum of squared error of (15), and is the th scene fusion experiment for the distance error of (16). So is the standard deviation of the 21-group fusion experiments. Then we do ten times independent experiments for the statistic performances of the PSO, and those 21-group different scenes are all tested in each independent experiment. Here Table 1 showed the standard deviations of evaluation function for the 21-group different scene in each independent experiment; the best extrinsic parameters calibration results are in the 10th experiment, the standard deviation of fitness function is only 0.2981, and the average of the fitness function is 0.3381, which is better than the nonlinear least square and nonlinear Gauss-Newton optimization methods for different constraints in [11]. Specifically, extrinsic parameters calibration is optimized by the nonlinear least square method for the first constraint (15); then these parameters are re-optimized by the nonlinear Gauss-Newton method for the second constraint (16) in [11]. In addition, the nonlinear least square and nonlinear Gauss-Newton methods are both utilized step by step for extrinsic parameters calibration process in [11].
However, the computational cost of particle swarm optimization algorithm is a little heavier than the methods in [11]; certainly, it also meets the requirements for the calibration reliability and real-time. As we all know that the intrinsic parameters are related to the inherent attribute of camera closely, the extrinsic parameters reflect the relative direction and position between the laser-rangefinder and the camera. After intrinsic and extrinsic parameters are calibrated, the sensor fusion process will not change those parameters. In other words, only if the camera is the same camera, the relative direction and position between laser-rangefinder and camera keep unchanged; then those calibration parameters also remain unchanged. Thus, we understand that the performance of calibration real time is not so significant as the performance of calibration accuracy for the calibration because we can calibrate parameters offline before the sensor fusion. In Table 1, the running time of 0.7154 s could be accepted by the sensor fusion process. Meanwhile, the optimization extrinsic parameters of each different experiment are all list in Table 1.
These mixed and separated intrinsic and extrinsic parameters are compared, respectively, for the laser-rangefinder and the camera data fusion. Figure 6 includes three examples of intrinsic and extrinsic parameters mixed method, which shows the apparent deviation errors in the black panes; however, parameters separated method leads to fewer errors, as shown in Figure 7. Meanwhile, the mapping effects of laser-rangefinder points onto the image are more reasonable with actual situation in Figure 7.
(a) The first example
(b) The second example
(c) The third example
(a) The first example
(b) The second example
(c) The third example
Additionally, the convergence curve for PSO to optimize the extrinsic parameters in the 10th experiment is given; we conclude that the standard deviation of the 21-group is always decreasing with iteration time increasing according to Figure 8. At last, it converges to 0.2981, which is much lower than the standard deviation from the methods in [11].
Furthermore, the best PSO calibration result of the tenth experiment is 0.2981, which is better than 0.3764. From the performance comparison between the PSO and the method in [11] for the optimization extrinsic parameters, we find that the majority of experiments by PSO for the optimization extrinsic parameters are better than the method in [11], except the 5th, 9th, 10th, 12th, and 15th experiment scene, as is shown in Figure 9, but the total standard deviation 0.2981 of PSO for extrinsic parameters calibration is better than 0.3764 of the method in [11], and the standard deviation of calibration error in the 21-group experiments is decreased by 10.175% compared with the method in [11].
Extrinsic parameters optimization is achieved by PSO; it considers two optimization objectives; the multiobjective calibration process is translated into a comprehensive objective. While the nonlinear least square is utilized for the first objective, the nonlinear Gauss-Newton method is utilized for the second objective in [11]; the theoretical evidence is not very sufficient, so the performance of calibration is not better for the sensor fusion.
At last, the calibration results for 21-group scene in the 10th independent experiment are shown in Figure 10; from those results, we can confirm that extrinsic parameters calibration based on PSO are valid and effective, especially in their segment boundaries.
(a) The 1st scene for experiment
(b) The 2nd scene for experiment
(c) The 3rd scene for experiment
(d) The 4th scene for experiment
(e) The 5th scene for experiment
(f) The 6th scene for experiment
(g) The 7th scene for experiment
(h) The 8th scene for experiment
(i) The 9th scene for experiment
(j) The 10th scene for experiment
(k) The 11th scene for experiment
(l) The 12th scene for experiment
(m) The 13th scene for experiment
(n) The 14th scene for experiment
(o) The 15th scene for experiment
(p) The 16th scene for experiment
(q) The 17th scene for experiment
(r) The 18th scene for experiment
(s) The 19th scene for experiment
(t) The 20th scene for experiment
(u) The 21st scene for experiment
6. Conclusion
According to the principle of heterogeneous calibration and the characteristics of a laser- rangefinder and a camera, the mapping relationship among world coordinate system, camera coordinate system, and image plane is discussed. Meanwhile, calibration algorithm takes into account separated intrinsic parameters and extrinsic parameters. Zhang’s algorithm is adopted to calibrate camera’s intrinsic parameters, and then the inherent properties of camera are analyzed. Moreover, the extrinsic parameters separated calibration and estimation based on PSO are proposed to improve the calibration’s accuracy and validity. Meanwhile, we design a characteristic line method to obtain extrinsic parameters estimation by two intersecting calibration boards with a certain angel. Furthermore, we applied PSO to optimize calibration parameters for two different objectives. And then the availability and reliability of a camera and a laser-rangefinder are insured by the calibration parameters separated and extrinsic parameters optimized based on PSO. In summary, the proposed separated parameters calibration and particle swarm optimization method for the camera and the laser-rangefinder in the paper are an improvement to traditional mixed calibration of intrinsic and extrinsic parameters; meanwhile, both the separated parameters calibration and the extrinsic parameters optimized based on PSO algorithm are to decrease the calibration error; furthermore, the experimental results and analysis indicate that the proposed calibration method can insure the accuracy and reliability of the camera and the laser-rangefinder information fusion.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
Acknowledgments
This paper was supported by the National Natural Science Foundation of China (Grant no. 61304253), Natural Science Foundation of Hunan (Grant nos. 13JJ4018 and 13JJ4093), and the Doctoral Program of Higher Education of China (Grant no. 20130162120018).
Supplementary Materials
In the experiment video, we show the calibration results of the proposed algorithm. at first, we load the laser rangefinder data, and then the images from camera are also loaded, which are the heterogeneous data at the same time. Secondly, the calibration window show the calibration results in real time, the red dots represents the laser rangefinder data, which are calibrated in the images from the camera. At last, the experiments proves the accuracy and effectiveness of our approaches.