Nowadays, robot platforms are typically equipped with multiple sensors, such as multiple cameras, 3D lidar sensors and IMUs, which require calibration in order to present sensed information in a common coordinate system. Cameras provide rich color and feature information, while lidar sensors can provide accurate distances. Cameras only work in appropriate lighting conditions and problems may happen at night. Lidars are active sensors, they emit infrared light and sense the reflections to detect objects in the environment, thus they can work in the dark. For robot tasks such as Simultaneous Localization and Mapping (SLAM) , it is important to know the relative pose of each sensor to the robot base and to each other.
Many approaches to calibrate different types of sensors have been proposed.
Calibration methods can be divided into online and offline calibration. Online calibration means that the relative poses of the sensors can be computed during the usage of the system. However, offline methods can usually provide more accurate extrinsic parameters.
proposed a method to estimate intrinsic and extrinsic parameters at the same time. There is a well-known Matlab calibration toolbox named Caltech camera calibration toolbox.
Another group is that of non-overlapping cameras. Kumar  utilizes mirrors to create virtual views of the calibration board. Such a method is flexible, however, it requires elaborate configuration. Carrera  utilizes an external SLAM system to compute extrinsic parameters. SLAM system based algorithms typically are not very accurate, because of wrong feature matching, especially in environments with repetitive structures. This will decrease the accuracy of the camera pose estimation.
Both lidar and cameras can provide rich and complementary data, which can be used in various tasks. For the transformation between 3D lidar and cameras, Dhall  utilizes a special target to compute extrinsic parameters. Correct 3D-3D corresponding points from the camera frame and the lidar frame are needed. Pandey 
utilizes a planar checkerboard pattern to compute the extrinsic parameters, with the assumption that the normal vector of the plane coincides with the normal vector of the 3D point cloud on the plane. They use these constraints to form a non-linear optimization problem, which was optimized by using the Levenberg Marquardt (LM)[9, 10] algorithm.
Martin  utilizes one planar board that has four circular holes in front of a blackboard. The holes in both the acquired image data and the 3D Lidar point cloud were detected automatically. This is flexible if the point cloud acquired from the 3D lidar is dense. However, we cannot apply this method to the point cloud measured by some 3D lidar sensors if the acquired 3D point is sparse. Ishikawa  proposes a method of target-less and automatic camera lidar calibration. Camera motion is estimated by feature detection and matching while lidar motion is calculated using the ICP  algorithm. The motion of 3D lidar may not be very accurate, since ICP algorithm is easy to fall into local optimum.
For multi-sensor calibration, Heng [14, 15] proposes an online multi-camera extrinsic calibration method using multiple view geometry and bundle adjustment. Sungdae  is using closed loop constraints for multi-sensor calibration.
, we also need one planar target. The two-stage estimation process is utilized to compute the translation part and the rotation part. In the second stage, a non-linear function is formed to jointly optimize the translation part and rotation part. For the calibration of two 3D lidars, we adopt a Normal Distribution Transformation NDT algorithm to compute the extrinsic parameters with an initial guess.
Hand-Eye calibration is a well know calibration problem. The goal of the conventional Hand-Eye calibration problem is to compute the transformation between a gripper (hand) and a camera (eye). One solution to this problem is to use bundle adjustment on identified feature points in the camera image and on the to-be-optimized transformation between the robot arm and the camera simultaneously .
The contributions of this paper are as follows:
We present a novel approach to calibrate multiple types of sensors based on graph optimization, with a focus on the accuracy of the extrinsic calibration.
We describe an extend hand-eye calibration method for non-overlapping cameras, especially also for the calibration of the camera w.r.t. our external, camera-based tracking system.
We present synthetic and real-data experiments to show the performance of our algorithm. Our mapping robot has 12 different sensors, which are calibrated well using our approach.
Ii Problem Description
An essential assumption for most calibration approaches is, that all sensors are rigidly mounted on the robot platform. Extrinsic calibration then means the estimation of the relative poses of sensor pairs, such that all the data collected from the different sensors can be fused into one single frame. Due to sensor noise, it is impossible to align the data without error. Also, when using real sensors, there is typically no way to accurately measure the transform (translation and rotation) of the physical sensor to another frame. This is because the sensor frame is typically somewhere inside the sensor, and because there are no tools available to measure arbitrary translations and rotations of physical objects with sufficient accuracy.
The sensor platform employed in this paper, the MARS Mapper (MARS is the acronym of our Mobile Autonomous Robotic Systems Lab), is shown in Figure 1. The MARS mapper features of two Velodyne HDL-32E lidar, nine FLIR Grasshopper3 cameras with 5 megapixel resolution, and one Xsens MTi-100 IMU. Among these twelve sensors, one of the lidar scans the horizontal plane and the other lidar scans the vertical plane. The cameras are mounted in pairs for stereo vision to the front, the left, the right and up. Additionally, there is a monocular camera facing backwards.
All cameras and the IMU are hardware synchronized using a custom micro-processor. This device also provides exact time-stamps to the Velodyne lidars, by simulating GPS time-stamps. Additionally, the system is also able to be hardware synchronized with the tracking system. Using a single Intel i7 CPU, the robot is able to gather and compress (JPEG Quality 90) the 5 MP image data of the 9 cameras with 10Hz, synchronized with the data-collection of the two lidars, the IMU and odometry data.
We first describe the pairwise calibration methods below and then the global multi-sensor calibration in Section III-B.
Iii-a Pairwise Calibration
The pairwise calibration is used to estimate the transformation between sensor pairs.
The following calibration pairs are considered and described in more detail below (the number of pairs for our real robot experiment is shown in parentheses):
Stereo-camera (overlapping) (4)
Non-overlapping cameras (32)
3D lidar to camera (13)
3D lidar to 3D lidar (1)
Tracking system to camera (9)
Non-overlapping camera pairs
We apply Hand-Eye calibration to calibrate non-overlapping cameras. Suppose we have a series of corresponding motions of the hand and the eye, and and are the homogeneous transformation of the eye and hand motions, respectively. Let be the transformation from hand to eye. We need at least two motions to formulate the Hand-Eye equation .
As each homogeneous transform has the following form
where and denote the rotation and translation, respectively.
We can split Eq. 1 into a rotation and a translation part.
We extend the Hand-Eye calibration to calibrate two non-overlapping cameras, since our mapping robot contains stereo cameras and also non-overlapping cameras. With two cameras rigidly mounted, we assume that one camera is the eye and the other camera is the hand. Having calibrated the intrinsic parameters beforehand, we can now estimate the motion of the cameras by using two checkerboards, one for each camera. Once we get a series of camera motions, the transformation of the two non-overlapping cameras can be estimated.
3D Lidar to Camera Calibration
The goal of our 3D lidar to camera calibration is to find the extrinsic parameters from the laser range coordinate frame to the camera frame. We assume the usual pin-hole camera model. The relationship between a homogeneous point in the world coordinates and its coordinate image position is given by:
where and are the extrinsic parameters related to the world coordinate frame and the camera coordinate frame. is the camera intrinsic matrix, a x orthonormal matrix representing the camera’s orientation. is a vector representing the camera’s position.
The laser range finder returns the distance measurements to the points on the planner board we use for calibration. Suppose in the laser range finder coordinate frame is located at in the camera coordinate frame. The transformation from the laser range finder coordinate frame to the camera frame can then be described by:
where and denote the rotation and translation, respectively.
For 3D lidar to camera calibration, we explore Hebert’s  calibration method. At first, the translation and rotation part are considered separately. In the second stage, a non-liner function is used to refine the translation and rotation part.
3D Lidar to 3D Lidar
Calibration between two 3D lidars can be considered as a registration problem for two 3D point clouds. With an initial guess, the NDT algorithm is applied to compute the relative pose of the two 3D lidars.
To check the calibration result of two 3D lidar, we adopted one planner board and calculate the normal of two 3D lidars in the frames named and , respectively. For the rotation part, suppose that is the transformation between the 3D lidars. For that we use the following formula:
Here must be close to if the rotation part of the calibration result is accurate. The closer to 1 D is, the more parallel the two normal vectors are, which is positively correlated with the rotation part of the calibration result. For the translation part, we utilize the following error function to verify the calibration result:
where and denote the rotation and translation, respectively. and are points from two lidars. The smaller is, the better the result.
Tracking System Sensors
To gather additional constraints for the later optimization we also collect calibration data using the tracking system. For this we attach several markers on the robot platform, which form a frame of reference rigid to the robot body. Now Hand-Eye calibration is applied to estimate the transformation from this tracking system frame on the robot to each of the sensors. The motion of the markers, and thus the frame of reference on the robot, can be precisely estimated by the tracking system, while the motions of cameras are estimated by the checkerboards.
Iii-B Global Calibration
Many existing approaches try to compute the transformation of a pair of sensors. Suppose we need to calibrate three sensors. The sensor and sensor pair is calibrated first. Repeatedly, we can calibrate the sensor and sensor pair. The transformation between sensor and sensor can be calculated by chaining the two previous calibrations, or it can be directly calibrated. In each calibration process, there is an error, which can be propagated to the next step.
The extrinsic parameters calculated by combining the two sensor pair transformations will differ from the transformation obtained from a direct calibration. A global calibration is needed to reduce the total errors. In the previous sections, we have computed the relative poses of each sensor pair. In the following we describe how to achieve a global calibration using those results.
We are building a hypergraph to do the global calibration, which contains several nodes and edges. The hypergraph used for calibrating multiple sensors is shown in Fig. 2. A spanning tree of the pairwise calibration results is used as the initial guess. In the big graph, we assume that all the sensors poses are unknown.
In this work, our hypergraph contains several nodes and hyperedges . A node contains the sensor pose while an edge denotes the relative transformation between two (sensor) frames. The goal of the graph optimization is to find the poses of that minimize all the errors over all edges which can fit all the available measurements. Define that node provides the poses where is the node identity. The hyper edge denotes the relation between the sensors. Then we can form the error function we want to minimize in the following form:
where is the initial constraint of node and . represents the information matrix of the constraint.
The graph optimization is aimed to minimize Eq. 10. During optimization the Jacobian of the error function will be computed. Then a linear step will be taken to find the poses of sensors which minimize the error of the function. With an initial guess , the solution of 10 can be found by iteratively solving a linear system with the system matrix and the vector , such that
. Here is the Jacobian of the error function, with an initial guess . To effectively solve the non-linear function we use G2O .
Iii-C Uncertainty of the Transform between each Pair
The uncertainty of transformation between each pair of sensors comes from two main sources. The uncertainty of the method we used to find the transformation in the pair-wise sensor calibration. The uncertainty of the data we collected from different sensors. In Hand-Eye calibration, the pose estimation of hand and eye have some uncertainties. Also, the method we used to compute the transformation contains some uncertainties. For different types of sensor pairs, the calibration algorithm can give us the best transformation between two sensor pairs. For convenience, we combine the transformation between one sensor pair into one vector .
The Jackknife resampling method 
is used to calculate the variance of. This method is performed by taking samples from the entire data pairs we collected to calibrate the two sensors. We use 3D lidar to camera calibration as an example. Suppose is the 3D lidar image pair, and is the total number of 3D lidar and image pairs we used for calibration. We omit the sample from the data for the Jackknife method. For each of these samples , a different parameter vector is obtained. The parameter variance is given by:
where , is the transformation between sensor pairs. The inverse of the variance matrix is the information matrix we used in our global optimization. The information matrix is , which is a diagonal matrix.
Iv Experimental Results
We evaluate our method on both synthetic data and real data. For the simulation tests we know the relative pose between the sensors. We implement our algorithm on G2O , a framework for graph optimization. For the real data tests, our mapping robot platform as shown in Fig. 1 is used to test our method. Bouguet’s camera calibration toolbox  is utilized to obtain the intrinsic parameters of the cameras.
Iv-a Experiments with Synthetic Data
The following experiments are designed to show that the proposed algorithm is able to work efficiently on multi-sensor calibration. We first evaluate our approach with synthetic data. A hyper-graph is build with vertices and edges. For all the edges between vertices, a zero-mean Gaussian noise is added to the transformation.
We generate datasets to evaluate the performance of our approach. The error of the graph is calculated by the sum of the squared errors between all edges. The rotation part between all edges and the translation part are considered separately. The error of the translation part is presented in terms of meters, so the error unit we shown in Fig. 5 is . For the rotation part we adopt angle-axis to represent the transformation. Fig. 5 and Fig. 5 show the translation error and rotation error after the global optimization result on the left. Additionally we see the noise that was added to the edges, which simulate the pair-wise calibration results. We can see that the variance of the global optimization is lower than the noise we add to sensor transformations.
Iv-B Experiments with Real Data
With the 9 cameras mounted on the MARS Mapper system, as shown in Fig. 1, there are 36 pairs of cameras. Among these 36 camera-pairs, four of those pairs are calibrated using stereo calibration. The two cameras in each stereo camera pair share a common field of view, which are calibrated with the algorithm proposed in . In addition, the other 32 pairs of non-overlapping cameras are calibrated using the Hand-Eye calibration described above. We collected camera data for these pairs with different poses of our mapping robot platform.
Besides, we also used our Optitrack tracking system 111https://www.optitrack.com/products/prime-13/ with 21 Prime 13 cameras fixed to a truss on the ceiling to calibrate the 9 cameras on the robot. For 3D lidar to camera calibration, we can only calibrate lidar to camera pairs that share a common field of view. Those are cameras with the horizontal 3D lidar and cameras for the vertical 3D lidar.
With the robot moving around, we can estimate the accuracy of two 3D lidar calibration results by using formulas 7 and 8. Once all the sensor pairs are calibrated, a hypergraph is built with vertices and edges. G2O is used to optimize the graph. In order to evaluate the optimized calibration result, the points acquired from the vertical 3D lidar are fused with the cameras that overlap its field of view. We propose an global optimization approach to optimization all the sensors. The following formula 15 is used to calculate the error.
where is Lie Algebra operation is the transformation between sensor and sensor , and are initial pose for sensor and sensor .
where is the global error. is the total number of edges in our graph, is the error for each edge. Fig. 6 shows the colored point cloud that was generated by fusing the range data of the lidar with the color data of the cameras, using the global, optimized calibration result. Table I shows that the error decreased after optimization.
|before optimization||after optimization|
Fig. 7 shows two example images from the data we collected with the cameras. In Fig. 7, the left image contains a color checker board. In Fig. 6 we can see the very well represented color board in the colored point cloud. Also, other parts of the point cloud look well calibrated. Green points in the point cloud indicate points which are not covered by any camera of the mapping robot.
In this paper, we presented a graph based algorithm for multi-sensor calibration of a mapping robot platform with many sensors. The proposed algorithm requires known sensor to sensor calibration before graph optimization. We estimate the covariance matrices of the hyper-edges of the graph using the Jacknife resampling algorithm. Experiments on synthetic data show quantitatively, that the optimization improves the calibration. Our real robot experiment demonstrates the good performance of our algorithm. In the future, we plan to add more sensor types such as IMU, 2D lidar and also the motion base of our robot to our hypergraph.
-  C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard, “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” IEEE Transactions on robotics, vol. 32, no. 6, pp. 1309–1332, 2016.
-  J. Heikkila and O. Silven, “A four-step camera calibration procedure with implicit image correction,” in cvpr. IEEE, 1997, p. 1106.
-  R. Tsai, “A versatile camera calibration technique for high-accuracy 3d machine vision metrology using off-the-shelf tv cameras and lenses,” IEEE Journal on Robotics and Automation, vol. 3, no. 4, pp. 323–344, 1987.
-  Z. Zhang, “A flexible new technique for camera calibration,” IEEE Transactions on pattern analysis and machine intelligence, vol. 22, 2000.
-  R. K. Kumar, A. Ilie, J. M. Frahm, and M. Pollefeys, “Simple calibration of non-overlapping cameras with a mirror,” in
-  G. Carrera, A. Angeli, and A. J. Davison, “Slam-based automatic extrinsic calibration of a multi-camera rig,” in 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011, pp. 2652–2659.
-  A. Dhall, K. Chelani, V. Radhakrishnan, and K. M. Krishna, “Lidar-camera calibration using 3d-3d point correspondences,” arXiv preprint arXiv:1705.09785, 2017.
-  G. Pandey, J. R. Mcbride, S. Savarese, and R. M. Eustice, “Extrinsic calibration of a 3d laser scanner and an omnidirectional camera,” IFAC Proceedings Volumes, vol. 43, no. 16, pp. 336–341, 2010.
-  K. Levenberg, “A method for the solution of certain non-linear problems in least squares,” Quarterly of applied mathematics, vol. 2, no. 2, pp. 164–168, 1944.
-  D. W. Marquardt, “An algorithm for least-squares estimation of nonlinear parameters,” Journal of the society for Industrial and Applied Mathematics, vol. 11, no. 2, pp. 431–441, 1963.
-  V. Martin, M. Z. Michal, and H. Adam, “Calibration of rgb camera with velodyne lidar,” in Int ernational Conference on Computer Graphics, Visualization and Computer Vision, 2014.
-  R. Ishikawa, T. Oishi, and K. Ikeuchi, “Lidar and camera calibration using motions estimated by sensor fusion odometry,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 7342–7349.
-  P. J. Besl and N. D. McKay, “Method for registration of 3-d shapes,” in Sensor Fusion IV: Control Paradigms and Data Structures, vol. 1611. International Society for Optics and Photonics, 1992, pp. 586–607.
-  L. Heng, B. Li, and M. Pollefeys, “Camodocal: Automatic intrinsic and extrinsic calibration of a rig with multiple generic cameras and odometry,” in 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2013, pp. 1793–1800.
-  L. Heng, M. Bürki, G. H. Lee, P. Furgale, R. Siegwart, and M. Pollefeys, “Infrastructure-based calibration of a multi-camera rig,” in 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014, pp. 4912–4919.
-  S. Sim, K. Kwak, J. Kim, and S. H. Joo, “Closed loop-based extrinsic calibration of multi-modal sensors,” in 2014 14th International Conference on Control, Automation and Systems (ICCAS 2014). IEEE, 2014, pp. 647–651.
-  R. Unnikrishnan and M. Hebert, “Fast extrinsic calibration of a laser rangefinder to a camera,” Robotics Institute, Pittsburgh, PA, Tech. Rep. CMU-RI-TR-05-09, 2005.
-  P. Biber and W. Straßer, “The normal distributions transform: A new approach to laser scan matching,” in Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003)(Cat. No. 03CH37453), vol. 3. IEEE, 2003, pp. 2743–2748.
-  X. Zhi and S. Schwertfeger, “Simultaneous hand-eye calibration and reconstruction,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 1470–1477.
-  K. Pathak, A. Birk, J. Poppinga, and S. Schwertfeger, “3d forward sensor modeling and application to occupancy grid based sensor fusion,” in 2007 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2007, pp. 2059–2064.
-  A. Birk, N. Vaskevicius, K. Pathak, S. Schwertfeger, J. Poppinga, and H. Buelow, “3-d perception and modeling,” IEEE robotics & automation magazine, vol. 16, no. 4, pp. 53–60, 2009.
-  C. Strecha, W. Von Hansen, L. Van Gool, P. Fua, and U. Thoennessen, “On benchmarking camera calibration and multi-view stereo for high resolution imagery,” in 2008 IEEE Conference on Computer Vision and Pattern Recognition. Ieee, 2008, pp. 1–8.
-  K. Daniilidis, “Hand eye calibration using dual quaternions,” The International Journal of Robotics Research, vol. 18, no. 3, pp. 286–298, 1999.
-  R. Horaud and F. Dornaika, “Hand eye calibration,” The International Journal of Robotics Research, vol. 14, no. 3, pp. 195–210, 1995.
-  R. Unnikrishnan and M. Hebert, “Fast extrinsic calibration of a laser rangefinder to a camera,” Robotics Institute, Pittsburgh, PA, Tech. Rep. CMU-RI-TR-05-09, 2005.
-  R. Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard, “g 2 o: A general framework for graph optimization,” in 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011, pp. 3607–3613.
-  M. H. Quenouille, “Approximate tests of correlation in time series,” Journal of the Royal Statistical Society, vol. 11, no. 1, pp. 68–84, 1949.
-  J.-Y. Bougue, “Camera calibration toolbox for matla,” http://www.vision.caltech.edu/bouguetj/calibdoc/index.html, 2004.