I Introduction
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) [1], 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.
The camera to camera calibration, it can be divided into two groups. One group requires an overlap between the two cameras. It is a well studied problem [2, 3, 4]. Zhang [4]
proposed a method to estimate intrinsic and extrinsic parameters at the same time. There is a wellknown Matlab calibration toolbox named Caltech camera calibration toolbox.
Another group is that of nonoverlapping cameras. Kumar [5] utilizes mirrors to create virtual views of the calibration board. Such a method is flexible, however, it requires elaborate configuration. Carrera [6] 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 [7] utilizes a special target to compute extrinsic parameters. Correct 3D3D corresponding points from the camera frame and the lidar frame are needed. Pandey [8]
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 nonlinear optimization problem, which was optimized by using the Levenberg Marquardt (LM)
[9, 10] algorithm.Martin [11] 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 [12] proposes a method of targetless and automatic camera lidar calibration. Camera motion is estimated by feature detection and matching while lidar motion is calculated using the ICP [13] algorithm. The motion of 3D lidar may not be very accurate, since ICP algorithm is easy to fall into local optimum.
For multisensor calibration, Heng [14, 15] proposes an online multicamera extrinsic calibration method using multiple view geometry and bundle adjustment. Sungdae [16] is using closed loop constraints for multisensor calibration.
In order to calibrate 3D lidar sensors and cameras, we exploit Ranjith’s [17] approach. Similar to Pandey [8]
, we also need one planar target. The twostage estimation process is utilized to compute the translation part and the rotation part. In the second stage, a nonlinear 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
[18] algorithm to compute the extrinsic parameters with an initial guess.HandEye calibration is a well know calibration problem. The goal of the conventional HandEye calibration problem is to compute the transformation between a gripper (hand) and a camera (eye)[3]. One solution to this problem is to use bundle adjustment on identified feature points in the camera image and on the tobeoptimized transformation between the robot arm and the camera simultaneously [19].
Using a fully calibrated sensor system it is possible to employ applications such as sensor fusion [20] and SLAM [1], especially in three dimensions [21].
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 handeye calibration method for nonoverlapping cameras, especially also for the calibration of the camera w.r.t. our external, camerabased tracking system.

We present synthetic and realdata 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 HDL32E lidar, nine FLIR Grasshopper3 cameras with 5 megapixel resolution, and one Xsens MTi100 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 microprocessor. This device also provides exact timestamps to the Velodyne lidars, by simulating GPS timestamps. 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 datacollection of the two lidars, the IMU and odometry data.
Iii Method
We first describe the pairwise calibration methods below and then the global multisensor calibration in Section IIIB.
Iiia 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):

Stereocamera (overlapping) (4)

Nonoverlapping cameras (32)

3D lidar to camera (13)

3D lidar to 3D lidar (1)

Tracking system to camera (9)
Stereocamera calibration
Nonoverlapping camera pairs
We apply HandEye calibration to calibrate nonoverlapping 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 HandEye equation [3].
(1) 
As each homogeneous transform has the following form
(2) 
where and denote the rotation and translation, respectively.
We can split Eq. 1 into a rotation and a translation part.
(3) 
(4) 
We extend the HandEye calibration to calibrate two nonoverlapping cameras, since our mapping robot contains stereo cameras and also nonoverlapping 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 nonoverlapping 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 pinhole camera model. The relationship between a homogeneous point in the world coordinates and its coordinate image position is given by:
(5) 
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:
(6) 
where and denote the rotation and translation, respectively.
For 3D lidar to camera calibration, we explore Hebert’s [25] calibration method. At first, the translation and rotation part are considered separately. In the second stage, a nonliner 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:
(7) 
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:
(8) 
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 HandEye 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.
IiiB 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:
(9) 
(10) 
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
(11) 
(12) 
. Here is the Jacobian of the error function, with an initial guess . To effectively solve the nonlinear function we use G2O [26].
IiiC 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 pairwise sensor calibration. The uncertainty of the data we collected from different sensors. In HandEye 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 [27]
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:(13) 
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 [26], 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 [28] is utilized to obtain the intrinsic parameters of the cameras.
Iva Experiments with Synthetic Data
The following experiments are designed to show that the proposed algorithm is able to work efficiently on multisensor calibration. We first evaluate our approach with synthetic data. A hypergraph is build with vertices and edges. For all the edges between vertices, a zeromean 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 angleaxis 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 pairwise calibration results. We can see that the variance of the global optimization is lower than the noise we add to sensor transformations.
IvB 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 camerapairs, 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 [4]. In addition, the other 32 pairs of nonoverlapping cameras are calibrated using the HandEye 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 ^{1}^{1}1https://www.optitrack.com/products/prime13/ 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.
(14) 
where is Lie Algebra operation is the transformation between sensor and sensor , and are initial pose for sensor and sensor .
(15) 
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  
error  0.0149  0.0126 
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.
V Conclusions
In this paper, we presented a graph based algorithm for multisensor 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 hyperedges 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.
References
 [1] 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 robustperception age,” IEEE Transactions on robotics, vol. 32, no. 6, pp. 1309–1332, 2016.
 [2] J. Heikkila and O. Silven, “A fourstep camera calibration procedure with implicit image correction,” in cvpr. IEEE, 1997, p. 1106.
 [3] R. Tsai, “A versatile camera calibration technique for highaccuracy 3d machine vision metrology using offtheshelf tv cameras and lenses,” IEEE Journal on Robotics and Automation, vol. 3, no. 4, pp. 323–344, 1987.
 [4] Z. Zhang, “A flexible new technique for camera calibration,” IEEE Transactions on pattern analysis and machine intelligence, vol. 22, 2000.

[5]
R. K. Kumar, A. Ilie, J. M. Frahm, and M. Pollefeys, “Simple calibration of
nonoverlapping cameras with a mirror,” in
Proc. IEEE Conference on Computer Vision and Pattern Recognition
, 2008, pp. 1–7.  [6] G. Carrera, A. Angeli, and A. J. Davison, “Slambased automatic extrinsic calibration of a multicamera rig,” in 2011 IEEE International Conference on Robotics and Automation. IEEE, 2011, pp. 2652–2659.
 [7] A. Dhall, K. Chelani, V. Radhakrishnan, and K. M. Krishna, “Lidarcamera calibration using 3d3d point correspondences,” arXiv preprint arXiv:1705.09785, 2017.
 [8] 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.
 [9] K. Levenberg, “A method for the solution of certain nonlinear problems in least squares,” Quarterly of applied mathematics, vol. 2, no. 2, pp. 164–168, 1944.
 [10] D. W. Marquardt, “An algorithm for leastsquares estimation of nonlinear parameters,” Journal of the society for Industrial and Applied Mathematics, vol. 11, no. 2, pp. 431–441, 1963.
 [11] 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.
 [12] 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.
 [13] P. J. Besl and N. D. McKay, “Method for registration of 3d shapes,” in Sensor Fusion IV: Control Paradigms and Data Structures, vol. 1611. International Society for Optics and Photonics, 1992, pp. 586–607.
 [14] 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.
 [15] L. Heng, M. Bürki, G. H. Lee, P. Furgale, R. Siegwart, and M. Pollefeys, “Infrastructurebased calibration of a multicamera rig,” in 2014 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2014, pp. 4912–4919.
 [16] S. Sim, K. Kwak, J. Kim, and S. H. Joo, “Closed loopbased extrinsic calibration of multimodal sensors,” in 2014 14th International Conference on Control, Automation and Systems (ICCAS 2014). IEEE, 2014, pp. 647–651.
 [17] R. Unnikrishnan and M. Hebert, “Fast extrinsic calibration of a laser rangefinder to a camera,” Robotics Institute, Pittsburgh, PA, Tech. Rep. CMURITR0509, 2005.
 [18] 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.
 [19] X. Zhi and S. Schwertfeger, “Simultaneous handeye calibration and reconstruction,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 1470–1477.
 [20] 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.
 [21] A. Birk, N. Vaskevicius, K. Pathak, S. Schwertfeger, J. Poppinga, and H. Buelow, “3d perception and modeling,” IEEE robotics & automation magazine, vol. 16, no. 4, pp. 53–60, 2009.
 [22] C. Strecha, W. Von Hansen, L. Van Gool, P. Fua, and U. Thoennessen, “On benchmarking camera calibration and multiview stereo for high resolution imagery,” in 2008 IEEE Conference on Computer Vision and Pattern Recognition. Ieee, 2008, pp. 1–8.
 [23] K. Daniilidis, “Hand eye calibration using dual quaternions,” The International Journal of Robotics Research, vol. 18, no. 3, pp. 286–298, 1999.
 [24] R. Horaud and F. Dornaika, “Hand eye calibration,” The International Journal of Robotics Research, vol. 14, no. 3, pp. 195–210, 1995.
 [25] R. Unnikrishnan and M. Hebert, “Fast extrinsic calibration of a laser rangefinder to a camera,” Robotics Institute, Pittsburgh, PA, Tech. Rep. CMURITR0509, 2005.
 [26] 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.
 [27] M. H. Quenouille, “Approximate tests of correlation in time series,” Journal of the Royal Statistical Society, vol. 11, no. 1, pp. 68–84, 1949.
 [28] J.Y. Bougue, “Camera calibration toolbox for matla,” http://www.vision.caltech.edu/bouguetj/calibdoc/index.html, 2004.
Comments
There are no comments yet.