MSG-Cal: Multi-sensor Graph-based Calibration

Jason L. Owens∗

Army Research LaboratoryPhilip R. Osteen∗

Engility CorporationKostas Daniilidis

University of Pennsylvania

Abstract—We present a system for determining aglobal solution for the relative poses between multiplesensors with different modalities and varying fields ofview. The final calibration result produces a tree oftransforms rooted at a single sensor that allows thefusion of the sensor streams into a shared coordinateframe. The method differs from other approaches byhandling any number of sensors with only minimalconstraints on their fields of view, producing a globalsolution that is better than any pairwise solution,and by simplifying the data collection process throughautomatic data association.

I. INTRODUCTION

Most researchers who work with robots are familiarwith the requisite yet error-prone process of determiningthe poses of multiple sensors in order to fuse the sensordata into a single reference frame. Therefore, a valuablegoal would be the creation of a system in which a singledata collection can automatically produce a globally op-timized calibration of an arbitrary sensor configuration.In this work we describe a method and system thatcomputes the relative poses for multiple environmentsensors with differing modalities and varied acquisitionrates using graph optimization.

A significant aim for sensor fusion research in roboticsis to obtain a calibration procedure that can be runinteractively and online using only the capabilities ofa mobile robot and its current environment. While analgorithm that does not require a calibration objectwould be ideal, the data association problem makes thisdifficult to achieve, which is made even more challengingby differing sensing modalities. Therefore, we constrainthe data association problem by making use of a cus-tom calibration object, shown in Figure 3a, with theexpectation that using the object will facilitate accuratecalibrations.

The system we present makes few assumptions aboutthe number, relative pose or fields of view of the sensors;in fact, the only requirement is that any single sensorhas a partially overlapping field of view with at leastone other sensor, so that the calibration object is alwaysobserved by at least one pair of sensors at the same time.The sensor type determines the representation of theobject that is detected and stored at each distinct poseof the target. We assume that the calibration target is

∗These authors contributed equally to this work.Corresponding author: jason.l.owens.civ@mail.mil

Fig. 1: The top image shows an example colored 3-D LRFframe from the calibration data collection.

planar, which allows us to construct the geometric rela-tionships between individual sensor detections requiredfor calibration. Finally, we assume that each sensor hasbeen intrinsically calibrated, and do not include intrinsicparameters as part of the problem formulation.

The calibration procedure can be divided into threephases: data collection, pairwise calibration, and graph-based global refinement. In short, the data collectionphase involves walking around the robot with the calibra-tion object, and using a remote trigger to indicate a framecapture. The frame is then transformed into a set of edgeswhere each edge represents a sufficient number of co-occurring detections between a pair of sensors. Given theedges collected during the previous phase, the pairwisecalibration phase computes the relative pose betweensensor pairs using RANSAC to filter outliers. Finally, theinliers from the previous phase are used to construct ahypergraph that uses non-linear optimization to generatea global pose graph solution.

Through both simulated quantitative results and real-world qualitative results, we show that the resultingframework successfully calibrates a number of differenttypes of sensors with minimal operator intervention, andthe global optimization is shown to be more accuratethan the initial pairwise calibration.

A. Related work

The fundamental challenge in sensor calibration isdetermining associations between each sensor’s data.Recent calibration approaches use either known scenegeometry (including specialized calibration targets), orattempt to calibrate in arbitrary environments. Systemsthat calibrate with no special scene geometry or cali-bration object, such as [1] and [2], require high qualityinertial navigation systems (INS) to compute trajectory.

Scaramuzza et al. are able to calibrate a 3-D laser and acamera without a calibration object or inertial sensors,by having a human explicitly associate data points fromeach sensor [3]. In contrast, our calibration frameworkuses only the data coming from the sensors we wishto calibrate, and does not require human assisted dataassociation.

Similar to the work of [4], [5], [6], [7], [8] and others,we also use a calibration object to facilitate automaticdata association. To our knowledge, there has been littlework in the calibration of an arbitrary number of varioustypes of sensors. Most work on calibration has beenlimited to either a single pair of different sensor types([4], [8], [5], [1], [3], [9], [7], [6]), or multiple instancesof a single type ([10], [2]). An exception is the workof Le and Ng [11], who also present a framework forcalibrating a system of sensors using graph optimization,though their graph structure differs fundamentally fromours; they require that each sensor in the graph be a 3-D sensor. Therefore, neither individual cameras nor 2-Dlaser scanners are candidates in their calibration. Insteadthey must be coupled with another sensor (e.g., couplingtwo cameras into a stereo pair) that will allow the newvirtual sensor to directly measure 3-D information. Theirresults agree with ours, that using a graph formulationreduces global error when compared with incrementallycalibrating pairs of sensors. Our approach is related tothe graph optimization proposed in [12] where the focusis on solving non-linear least squares on a manifold.Our approach provides initial estimates from pairwisesolutions, and it has been tested on a graph of five sensorswith three different modalities.

B. Paper contributions

We describe a flexible framework for accurately cali-brating and fusing data from an arbitrary configurationof camera, laser, and point cloud sensors, where initial-ization estimates are generated automatically from thecollected data. In addition, our system for collecting datarequires only one operator, who needs no domain-specificexpertise and need not tediously associate data points.We describe a unique formulation of the calibrationproblem as a hypergraph containing both sensors andobservations as separate vertices, incorporating geomet-ric constraints between the different detection modalities.Finally, we developed a simulation system that producesall three types of data used for the calibration andcan be used to produce benchmark data sets for futurecalibration research.

II. PROBLEM DESCRIPTION

We address the extrinsic calibration of multiple sensorswith various modalities and acquisition timescales thatare rigidly mounted to a robotic vehicle.

In this context, calibration means the estimation ofrelative sensor poses such that features detected by mul-tiple sensors can be fused into a single coordinate frame.

Fig. 2: The robot sensors we calibrate in this paper.The image highlights the three 2-D Hokuyo LRFs, thespinning Hokuyo 3-D LRF, and the Ladybug5 sphericalcamera.

Due to sensor noise and systematic feature estimationuncertainty, it’s unlikely that features will align withouterror, so the process must be constructed to mimimizethe error across all sensors.

In our case, we consider visible light cameras, 2-D planar laser range finders (LRFs) such as theHokuyoTMUTM-30, and 3-D laser range scanners (suchas the VelodyneTMHDL-32), or moving 2-D LRFs such asthe one shown in Figure 2. We work with two geometricobjects over the three sensor types: 3-D lines derivedfrom 2-D LRFs and 3-D planes derived from both thecameras and 3-D LRFs. The calibration target itself is alarge planar poster (as seen in Figure 3a) with fiducialtags for the cameras. Section III describes the objectdetection methodology for each sensor type.

The rest of the paper is organized as follows: the sub-sequent three sections (III-V) describe each phase of theprocedure in detail. We briefly describe the simulationwe used to produce quantitative results in section VI.We then discuss the experimental results in section VIIand finally conclude in section VIII.

III. DATA COLLECTION

Data collection proceeds by launching a capture pro-cess with access to the robot’s sensor streams. Theuser physically places the calibration object in variouspositions around the robot to capture sufficient viewsacross all the sensors. Unfortunately, the sensors weare calibrating do not all support time synchronization.Therefore, to effectively bypass the problem of temporaldata association, we require the user to hold or placethe board in a fixed position until all sensors have takenat least two measurements. The requirement for twomeasurements is for sensors with long integration times(several seconds), where one cannot be assured the boardwas not in motion during the capture of the first reading.This implies that the time the calibration object mustremain still is at most twice the period of the slowestintegrating sensor, which for most sensors will be less

than one second. Thus, there is no need to specify thedata rates, or provide synchronization between the sensoracquisition processes.

We use the term message to refer to a single logicaldatum from a sensor, i.e. an image from a camera, ascan from a 2-D LRF, or a point cloud from a 3-D LRF.The term frame is used to refer to a collection of messagesfrom every sensor. When the calibration object is in placefor a new frame, the user sends a trigger to the systemand data collection begins.

Each frame collected contains all the sensor data.While it may be possible that every sensor observes thecalibration object, more likely only two or more sensorssee it at one time. We must detect and extract thecalibration object geometry for each sensor in order tocorrectly produce the edges in the sensor graph. At thisstage, the system has no knowledge of the relative sensorpositions, nor of the location of the calibration object inthe environment, so given an image, point cloud or laserscan, the system must automatically detect the objectfor each sensor if it is in view.

A. Background subtraction

Since we rely on explicit correspondences betweencalibration object detections across sensors, we wouldlike to have some assurance that the object we detect foreach sensor in a frame is in fact the calibration object.We solve this problem using a background subtractionapproach, which practically eliminates the data associ-ation problem and avoids constraints related to sensormodality.

At the very beginning of data collection, we allowthe point cloud and line sensors to build up a modelof the background environment in order to subtract itfrom the live dynamic data generated by the calibrationtarget. This technique is typically used in imaging sys-tems for surveillance, object tracking, and backgroundreplacement [13]. In our algorithm, we use an analogoustechnique for 3-D point clouds provided by PCL [14]that takes advantage of a modified octree. This octreeis a spatial data structure that hierarchically subdividesvolumes into octants and also provides efficient changedetection. We accumulate points from the 3-D sensorsover a short period of time, and store the observationsin the octree. When data collection begins we createa new octree for each message from the sensors, andthen subtract the accumulated background, leaving onlythe calibration operator and target (and possibly a fewother noisy points) in the scene. Due to our usage ofAprilTag fiducials on the calibration object, we do notneed any explicit technique for background subtractionfor cameras, since the tag detection automatically takescare of that.

B. Line and plane extraction from point clouds

Given a sparse point cloud from the background sub-traction process, we use the following process to find the

calibration object plane. First, sparse points are removedusing a statistical filter, normals are computed for eachpoint using the local neighborhood and then clusters arediscovered by growing regions seeded by a point withthe minimum curvature. For each cluster, the largestplane is extracted using random sampling consensus. Theplane that best matches the known dimensions of thecalibration object is selected as the valid detection forthis sensor. In practice, there are few clusters due to thebackground extraction (usually related to the user’s bodyor other stray points in the environment that may nothave been captured by the background accumulation),and all but one of these clusters is too small to be theobject.

Line extraction from the 2-D laser range finders pro-ceeds in much the same way as the plane extraction,however, instead of using a plane model, we use a linemodel with RANSAC. For each detected line, foundusing loose thresholds to enable high recall, we ensureit is contiguous and within the required size bounds.Finally, we filter the egregious outliers, then refit a lineto the remaining points. The endpoints we use to definethe detected line segment are determined by projectingthe actual scan endpoints to the closest point on themodel line. The resulting line segment is reported as thedetection for this sensor.

C. Plane extraction from AprilTag

As described in section II, the calibration target isprimarily a planar object used to induce lines and planesfrom 2-D and 3-D LRFs, but we need to also be ableto detect the plane from a camera. In the general case,cameras are projective devices, and can only determinean object pose up to scale. The AprilTag algorithmutilizes the known size of the tag, planar homography,and the camera parameters to determine the 3-D poseof each tag. We utilize three tags for redundancy, anduse as many tags as are visible to determine the planeparameters. If more than one tag is visible, we currentlycompute the average normal and distance to origin.

IV. PAIRWISE CALIBRATION

The pairwise calibration phase estimates the SE(3)transforms between each pair of sensors with co-occuringdetections, using RANSAC to filter outliers. Since weconvert all detections into the geometric primitives ofeither planes or lines, then in order to estimate thetransform between each pair, we must solve one of thefollowing objective functions.

A. Plane to plane

We use the Hessian normal form of a plane Pi =ni, di where nT

i x = −di. Therefore, if x is a pointon plane Pi, then nT

i x + di = 0. As in [15], we notethat the rotation and translation are separable problems.The normals are related by the rotation iRj alone:ni =

iRjnj . Using the plane equation, we can define therelations:

nTi (

iRjxj +itj) = −di

nTi

iRj(−djnj) + nTi

itj = −di−djnT

i (iRjnj) + nT

iitj = −di (1)

nTi

itj + di − dj = 0, (2)

where, in Equation 1 we utilize the fact the nTi

iRjnj ≈ 1,since the corresponding normals are unit vectors and iRj

brings nj into the frame of ni.We use these constraints in the following objective

function:

min(iRj ,itj)∈SE(3)

∑(Pi,Pj)∈C

∥∥ni − iRjnj

∥∥+∑

(Pi,Pj)∈C

[nTi

itj + di − dj]2, (3)

where C is the set of plane correspondences for a sensorpair. In our implementation, we use the Kabsch algo-rithm [16] for estimating the rotation between the normalsets. To determine the translation, we compute the leastsquares result of Equation 2. In this paper, we treat allthe normals with equal weight. However in the future weplan to compute the uncertainty of the detected planesand use a variant of Wahba’s algorithm to compute theoptimal rotation as per the method discussed in [17].

B. Plane to line

In contrast to our previous work [15], we utilize anobjective function that minimizes the distance of thesegment endpoints to the corresponding plane:

min(R,t)∈SE(3)

∑(Pi,`i)∈C

∑xji∈`i

[nTi x

ji + di

]2(4)

While we provide no initial estimate, we have discov-ered that the line to plane RANSAC algorithm performsbest with a sample size of five in order to sufficientlyconstrain the degrees of freedom.

V. GLOBAL CALIBRATION

We now describe the graph optimization that achievesthe global calibration. In the previous phase, we computea pairwise relative pose for every pair of sensors that meetor exceed the minimum number of observations. In thisphase, we construct a hypergraph composed of severalnode and edge types that exploit the pairwise relativetransforms as an initialization for the global sensor posegraph. The distinction is subtle but important: in therobot frame, we must pick one sensor as the root of thetransform hierarchy (most transform libraries, e.g. tf inROS, require a transform tree), and then connect theother sensors to this root, as per the pairwise connectionsin the previous phase, effectively forming a spanningtree over the graph of sensors. In addition, the pairwisecalibration phase uses no additional information (i.e.other co-occuring detections) in order to minimize the

observation error over more than one path in the graph.The goal of the global graph approach is to incorporateall the information into a single optimization structure,courtesy of g2o [18]. In our formulation, the sensor posesare the unknowns we wish to estimate simultaneouslyin a global frame. We let the user pick one sensor thatwill act as the root of the graph (e.g. a sensor withthe largest FOV) and therefore become the origin of theglobal frame. In order to estimate the initial poses, weconstruct a minimum spanning tree TS based on the edgeweight between the sensors, defined as the sum of thesquared errors computed during the pairwise calibration.

In this work, our hypergraph G is defined as a setof nodes V and hyperedges E. We include three nodetypes in V = S ∪ L ∪ P: sensor (S), line-observation(L), and plane-observation (P). The sensor node setS ⊂ SE(3) contains elements xS ∈ S, the unknownsensor poses, initialized as per the sensor-sensor trans-form computed from the spanning tree TS. The line-observations and plane-observations correspond to everyinlier detection found during the pairwise calibrationphase. Line-observations L ⊂ R3 ×R3 are elements xL ∈L, the endpoints of the detected lines. Plane-observationsP ⊂ R4 are elements xP ∈ P, i.e. the normal and distanceto the origin. Note that L and P nodes exist relative totheir observing sensors, while the sensors are the onlyentities in the global frame (where the root of TS is takenas the origin).

The objective of our graph optimization is to findthe sensor pose set S that minimizes the error overall edges in the graph. One can think of the nodesxαi ∈ V, α ∈ S,P,L as providing the data values

where i is the node identity and α is the node type.The hyperedges eγ ∈ E provide relations on the data,where γ ⊂ i : xα

i ∈ V . The edges are the observationsfrom the sensors and we construct functions Feγ thatrepresent the noisy constraints in the system. Since thegraph contains three node types there are necessarily sixcorresponding binary edge types, however, we currentlyonly make use of five since we have not yet implementeda line-to-line pairwise calibration procedure.

The functions Feγ compute the errors we wish to min-imize, representing the degree to which the parametersxαi , . . . ,x

βj satisfy the initial constraint µγ (computed as

the edge type-specific sensor observation). Ωα,β repre-sents the information matrix of the constraint.

minS

∑eγ∈G

Feγ (xαi , . . . ,x

βj , µγ)

>Ωα,βFeγ (xαi , . . . ,x

βj , µγ)

(5)

The graph optimization is implemented as a sparsenon-linear least-squares minimization over the error val-ues produced by the graph edges as shown in Eq. 5. Dur-ing optimization, the algorithm computes the Jacobiansof the error functions, and takes small linear steps inthe tangent space of the sensor node manifold, in order

to distribute the error over the nodes as defined by theiruncertainty. Each node type provides an appropriate stepoperator in the tangent space, and uses the exponentialmap as necessary to compute the corresponding point onthe manifold.

In the following sub-sections, we define each errorfunction Feγ given the edge type defined by the set ofobservation types in each node.

Sensor-sensor edge . The sensor-sensor edge repre-sents the relative transform between the sensor nodesit relates. Our error is defined as the difference of therelative transform from the mean. We define the meanto be the initial relative transform we compute fromthe pairwise calibration phase for this pair of sensors.If µij ∈ SE(3) is the initial relative transform betweensensor pair xS

i , xSj , then the edge error in the tangent

space is defined as:

logSE(3)(µ−1ij ((xS

i )−1xS

j)). (6)

Sensor-line edge . The sensor-line edge representsthe measured line endpoints relative to the sensor’scoordinate frame. This edge constrains the adjustment ofthe line in the sensor frame with respect to the originalobservation. The edge uncertainty is related to the noiseinherent in the laser scan, i.e. we assume the points(r, θ) detected by the 2-D LRF are perturbed by twoindependent normally distributed variables a, b as in:

pi = (r + a, θ + b) (7)

a ∼ N (0, σr) (8)

b ∼ N (0, σθ). (9)

The error is defined in a six dimensional space (R3×R3),with extremely low uncertainty in the z coordinate foreach point. If µij is the initially observed endpoints ofthe line j from sensor i, then the error is xL

j − µij .Sensor-plane edge . The sensor-plane edge is defined

similarly to the sensor-line edge, but is parameterizedby the four dimensional space (R3 ×R) representing theplane parameters. Given µij as the initially observedplane parameters, the error is xP

j − µij .Observation-observation edges . The following two

hyperedges correspond to the unique constraints formedby the interaction between the geometric primitives.Each hyperedge is structured in the following manner:

where the squares are the observation nodes (line,plane)and the circles are the sensor nodes. The black pentagonrepresents the hyperedge. In every case, the observationsare in the local frame of the corresponding sensor, whichrequires that we transform one of the objects into theframe of the other sensor. This is achievable since thesensor nodes represent their pose estimate in the globalframe. We can compute the relative transform betweenthe sensors and use this to bring the second observationinto the frame of the first.

Line-plane edge . The line-plane edge connects a lineand plane as detected by the respective sensor. In thiscase, we use the plane definition to represent the errorin R2 as the offset of each of the line endpoints to theplane.

Plane-plane edge . Finally, the plane-plane edge con-nects the observations of the same plane from two differ-ent sensors. Given the transform of P2 into the frameof P1, we define the error in R4 as (a1, b1, c1, d1) −(a2, b2, c2, d2), where a, b, c, d are the plane parameters.

VI. SIMULATION

In order to provide a benchmark quantitative evalua-tion of the proposed calibration procedure, we developeda simulation that generates the same raw sensor mes-sages as the robot yet allows us to specify the groundtruth poses for the sensors. The simulation includes ageometric representation of the calibration object derivedfrom the generating PostScript program and realisticimplementations of the target sensors: a pinhole camerathat produces images of the object, a 2-D laser rangefinder to generate point clouds from the (r, θ) scan, anda spinning 2-D laser range finder that produces 3-Dpoint clouds. We briefly describe the data set generationprocess, then follow with the implementation details ofthe sensors.

A. Data set generation

Similar to the collection procedure described in SectionIII, we generate randomly distributed poses for the cal-ibration object, assuming the sensor system is situatedat the origin. The procedure allows one to specify thebearing θ, radius r, maximum rotation around an axisφ and the number of random calibration object rotationsamples to generate at each pose (θ, r),

R(θ,r) =(ψr, ψp, ψy)i | ψr ∼ U(−φ, φ),

ψp ∼ U(−φ, φ), ψy ∼ U(−φ, φ).

For each sample, we “expose” the transformed cali-bration object to each simulated sensor and record theobservations in a bag file.

B. Pinhole camera

The camera is modeled by the width and height of thegenerated image and the camera matrix:

K =

fx 0 cx0 fy cy0 0 1

. (10)

We make the assumption that the camera is intrinsicallycalibrated, and do not generate any distortion. The imagegeneration process is as follows: (1) a high resolutionimage of the calibration object is generated (i.e. 3 pixelsper mm) directly from the metric specification of theposter size and embedded AprilTags. (2) Boundary co-ordinates are extracted from the transformed points of

(a) (b)

Fig. 3: (a) An image generated from a rotated calibrationobject at 2 m. (b) An example laser scan and point cloudrendered in 3-D before calibration, along with the groundtruth object

the calibration object, and are projected onto the imageplane using the projection π : R3 → R2:

π(x) =

(fxx1x3

− cx,fyx2x3

− cy

). (11)

Given the real image coordinates, we compute the ho-mography matrix H that transforms the high-resolutionmodel image to the planar deformation in the projectedimage. An example image is shown in Figure 3a.

C. 2-D laser range finder

We implement the 2-D LRF by intersecting rays castfrom the origin of the sensor with the plane of thecalibration object. Assuming the origin of the sensor ispo, we construct a set of rays ρiN1 (normalized vectors)emanating from po with a configured angular resolution,start, and end angle. For each scan, we perturb the anglesused to generate the rays, and once a valid intersection isfound, we perturb the true radius according to Equation9. We use the following ray intersection equation:

n · (ρit+ po) + d = 0 (12)

n · ρit = −n · p0 − d (13)

t =−n · po − d

n · ρi, (14)

and discard intersections where t < 0, since that meansintersection is behind the laser. With a value for t wecan compute the point on the plane, and then determinewhether it is within the calibration object bounds. Thisis accomplished by testing for negative intersections withall four half-planes defined by the boundary segmentnormals. These normals lie orthogonal to the plane nor-mal and boundary segment, and emanate away from thecentroid. Figure 3b shows an example laser scan in 3-D.

D. 3-D laser range finder

The 3-D LRF is implemented by “spinning” the 2-D LRF around a vertical axis and therefore generatingmultiple 2-D laser scans from different poses. We mimicthe construction of the sensor shown in Figure 2, with thefollowing parameters: tilt, offset, scans per degree, andscan angle. The first two parameters control the structure

Fig. 4: The structure of the 3-D laser range finder. θindicates the tilt (away from positive z), while d controlsthe offset from the axis of rotation, positive z. The shadedregion indicates the vertical scan pattern from the 2-DLRF.

of the sensor, while last two parameters determine thedensity of the scans and the field of view (see Figure 4for an illustration of the geometry). If one sets the offsetand tilt both to zero, then the effect would be to simplyrotate a 2-D LRF with the forward axis parallel to thez axis. Since the pose of the scan plane changes as theLRF rotates around the axis, we must transform eachscan into the frame of the origin. Doing so generates theblue cross-hatched plane shown in Figure 3b.

VII. EXPERIMENTAL RESULTS

We evaluate the proposed system on two sets of sen-sors: one in which we know the ground truth, and canprovide quantitative error metrics with respect to com-puted vs. actual position, and a set of sensors mountedon a mobile robot, in which we do not have ground truth.The calibration system is implemented as a set of C++and Python packages based on the ROS ecosystem. Thesimulated and real datasets contain 120 and 133 differentcalibration object poses corresponding to one frame perpose, respectively.

The robot is a ClearPath HuskyTMwith five primarysensors as shown in Figure 2. All three modalities arerepresented, including several 2-D LRFs, one slowly ro-tating 3-D LRF, and five cameras we use as one camerafrom the Ladybug5 spherical camera.

A. Error computation

We report our results using the following two metrics:error over the graph and deviation from ground truth(for the simulation study). The graph error is currentlydefined as the sum of the squared error over all edgesother than the sensor-sensor edges (i.e. every sensor-observation and observation-observation edge). Thus,this error is related to all the data embedded in thegraph. Every error is represented in terms of meters,so the error unit is m2. For the simulation, we directlycompare the sensor pose model with the resulting posesfrom the optimization procedures. We report these errorsas the mean translational and angular offset (measuredas the smallest angle between the principal axes of two

sensors, i.e. the angle subtending the geodesic on the unitsphere S2) from ground truth over all sensors.We also investigated several variations on the algo-

rithm to determine performance under different condi-tions. In particular, we were interested in the value ofthe pairwise calibration phase. First, we compared theglobal result when the sensors were initialized from thepairwise spanning tree transform vs. using the identitytransform, and discovered this yielded no significantdifference. However, in both cases the inlier set from thepairwise calibration was used. We wondered whether theresults would be the same if we used all the data in thegraph (no filter) vs. using the pairwise RANSAC inlier set(SAC filtered). The following two sections summarize ourresults for the simulation and real-world robot sensors.

B. Simulation

We use the simulation to show the algorithm workswith respect to ground truth, given realistic simulatedsensor data. Figures 5a and 5d show the benchmarkerror of the simulation configuration against the knownground truth sensor poses. One thing to note is that thevariance of the graph solution is always lower than justthe pairwise pose results over multiple runs. In addition,it is interesting to note that the mean of the angularerror is lower than the graph (by 0.03 degrees), but thisis not entirely unexpected given the error over the wholesystem: the pairwise solution represents the best relativepose given the data, but the global optimization has tocorrect for these errors across the entire graph. In Figure5b, we see that the global graph error mean and varianceare reduced as compared to the best pairwise solutionwith no global optimization. Finally, Figure 5c illustratesthe performance over multiple runs while comparingthe global optimization performance with and withoutfiltering the outliers using the pairwise calibration phase.Note that while the filtered error shows small variance,the error is significantly lower when the outliers arefiltered.

C. Robot Sensors

To test the algorithm using real-world sensors, wecollected data from the robot and sensors shown inFigure 2. Since we do not have ground truth, we reportthe results of the graph error given the pose estimatesfrom the pairwise calibration as compared to the errorafter the global graph optimization (Figure 5e). We alsoshow the difference between filtering the outliers andusing all the data with no filtering (Figure 5f). Note theagreement between the relative values as compared tothe simulation results; graph optimization improves theerror over the pairwise initialization, given the structureof the edge constraints. In addition, filtering the outlierssignificantly improves the error result.

However, we feel the qualitative results speak moreclearly to the capability of the algorithm, and we showtwo screen captures from the 3-D visualization of the

fused data. Figure 6a shows a large indoor area capturedwith the system with an inset of the corresponding imagefrom the Ladybug’s cameras. Figure 6b shows anotherscene from a highly cluttered machine shop in the samebuilding, with small details that reinforce the practicalperformance of the system.

VIII. CONCLUSION

We have presented an end-to-end system for calibrat-ing a set of minimally constrained multi-modal sensorsrigidly mounted to a robot. Although many of the con-cepts have been presented in previous works, as far aswe are aware this is the first algorithm to bring themtogether into a cohesive graph optimization frameworkpaired with a simplified data association process. In thefuture, we plan to evaluate the algorithm on a wider arrayof data sets and explore improvements to the cameraplane estimation algorithm. In addition, we will work tomake the code available as an Open Source distributionfor ROS so others can benefit and improve upon thiswork.

References

[1] J. Levinson and S. Thrun, “Automatic online calibration ofcameras and lasers,” in Robotics: Science and Systems, 2013.

[2] W. Maddern, A. Harrison, and P. Newman, “Lost in transla-tion (and rotation): Rapid extrinsic calibration for 2d and 3dLIDARs,” in Robotics and Automation (ICRA), 2012 IEEEInternational Conference on, pp. 3096–3102, IEEE, 2012.

[3] D. Scaramuzza, A. Harati, and R. Siegwart, “Extrinsic selfcalibration of a camera and a 3d laser range finder from naturalscenes,” 2007.

[4] Q. Zhang and R. Pless, “Extrinsic calibration of a camera andlaser range finder (improves camera calibration),” in Intelli-gent Robots and Systems, 2004. (IROS 2004). Proceedings.2004 IEEE/RSJ International Conference on, vol. 3, (Sendai,Japan), pp. 2301–2306, Oct. 2004.

[5] P. Nunez, P. Drews Jr, R. Rocha, and J. Dias, “Data fusioncalibration for a 3d laser range finder and a camera usinginertial data.,” in ECMR, pp. 31–36, 2009.

[6] R. Unnikrishnan and M. Hebert, “Fast extrinsic calibration ofa laser rangefinder to a camera,” Tech. Rep. CMU-RI-TR-05-09, Robotics Institute, July 2005.

[7] H. Li and N. Fawzi, “Comprehensive extrinsic calibration ofa camera and a 2d laser scanner for a ground vehicle,” p. 24,July 2013.

[8] F. M. Mirzaei, D. G. Kottas, and S. I. Roumeliotis, “3dLIDAR-camera intrinsic and extrinsic calibration: Identifia-bility and analytical least-squares-based initialization.,” I. J.Robotic Res., vol. 31, no. 4, pp. 452–467, 2012.

[9] F. Vasconcelos, J. P. Barreto, and U. Nunes, “A minimalsolution for the extrinsic calibration of a camera and alaser-rangefinder,”Pattern Analysis and Machine Intelligence,IEEE Transactions on, vol. 34, no. 11, pp. 2097–2107, 2012.

[10] S. Miller, A. Teichman, and S. Thrun, “Unsupervised extrinsiccalibration of depth sensors in dynamic scenes,” in Proc. of theIEEE/RSJ International Conference on Intelligent Robots andSystems (IROS), 2013.

[11] Q. V. Le and A. Y. Ng, “Joint calibration of multiplesensors,” in Intelligent Robots and Systems, 2009. IROS2009. IEEE/RSJ International Conference on, pp. 3651–3658,IEEE, 2009.

[12] R. Wagner, O. Birbach, and U. Frese, “Rapid developmentof manifold-based graph optimization systems for multi-sensor calibration and SLAM,” in Intelligent Robots and Sys-tems (IROS), 2011 IEEE/RSJ International Conference on,pp. 3305–3312, IEEE, 2011.

global error pairwise error0.002

0.004

0.006

0.008

0.010

0.012

0.014

0.016

0.018

Avera

ge e

rror

(m)

Simulation: Pose translation benchmark error

(a)

global error pairwise error0.00345

0.00350

0.00355

0.00360

0.00365

0.00370

0.00375

0.00380

0.00385

RMS

erro

r (m

)

Simulation global error vs pairwise error

(b)

trials0.0034

0.0035

0.0036

0.0037

0.0038

0.0039

0.0040

0.0041

0.0042

RMS

erro

r (m

)

Simulation outlier filter vs no filter

SAC filterNo filter

(c)

global error pairwise error0.000

0.002

0.004

0.006

0.008

0.010

0.012

Avera

ge e

rror

(radia

ns)

Simulation: Pose rotation benchmark error

(d)

global error pairwise error0.0064

0.0066

0.0068

0.0070

0.0072

0.0074

0.0076

RMS

erro

r (m

)

Husky global error vs pairwise error

(e)

trials0.00

0.01

0.02

0.03

0.04

0.05

0.06

0.07

0.08

RMS

erro

r (m

)

Husky outlier filter vs no filter

SAC filterNo filter

(f)

Fig. 5: (a) Simulation mean translation benchmark error (poses computed against ground truth). (b) Simulationgraph error comparing the best pairwise error result vs. global error. (c) Simulation global graph error comparingresults with and without filtering the data using the sampling consensus. (d) Simulation mean rotational benchmarkerror (poses computed against ground truth). (e) Husky graph error comparing the best pairwise error result vs.global error. (f) Husky global graph error comparing results with and without filtering the data using the samplingconsensus.

(a) Indoor, open area scan. (b) Indoor, cluttered area scan.

Fig. 6: Fused sensor output using the global calibration results for the Husky sensor data collection. The Ladybugimages are back-projected to the point cloud, given the relative transform between the two sensors. Also note theblue, purple, and cyan points that show the transformed 2-D laser scan output. In particular, (b) shows how wellthe laser scans align to the camera and 3-D LRF, e.g. by their intersection on the handcart on the left side of theimage.

[13] P. KaewTraKulPong and R. Bowden, “An improved adaptivebackground mixture model for real-time tracking with shadowdetection,” in Proc. 2nd European Workshop on AdvancedVideo Based Surveillance Systems, vol. 25, pp. 1–5, 2001.

[14] R. B. Rusu and S. Cousins, “3d is here: Point cloud library(PCL),” in IEEE International Conference on Robotics andAutomation (ICRA), May 2011.

[15] R. Tron, P. Osteen, J. Owens, and K. Daniilidis, “Pose opti-mization for the registration of multiple heterogeneous views,”in Workshop on Multi-View Geometry in Robotics, 2014.

[16] W. Kabsch, “A discussion of the solution for the best rotation

to relate two sets of vectors,” Acta Crystallographica Sec-tion A: Crystal Physics, Diffraction, Theoretical and General

Crystallography, vol. 34, no. 5, pp. 827–828, 1978.[17] J. Poppinga, N. Vaskevicius, A. Birk, and K. Pathak, “Fast

plane detection and polygonalization in noisy 3d range im-ages,” in Intelligent Robots and Systems, 2008. IROS 2008.IEEE/RSJ International Conference on, pp. 3378–3383, 2008.

[18] R. Kummerle, G. Grisetti, H. Strasdat, K. Konolige, andW. Burgard, “G2o: A general framework for graph optimiza-tion,” in 2011 IEEE International Conference on Robotics andAutomation (ICRA), pp. 3607–3613, IEEE, May 2011.