arxiv: v1 [cs.cv] 13 Sep 2017

Transcription

1 A General Framework for Flexible Multi-Cue Photometri Point Cloud Registration Bartolomeo Della Corte Igor Bogoslavskyi Cyrill Stahniss Giorgio Grisetti ariv: v1 [s.cv] 13 Sep 2017 Abstrat The ability to build maps is a key funtionality for the majority of mobile robots. A entral ingredient to most mapping systems is the registration or alignment of the reorded sensor data. In this paper, we present a general methodology for photometri registration that an deal with multiple different ues. We provide examples for registering RGBD as well as 3D LIDAR data. In ontrast to popular point loud registration approahes suh as ICP our method does not rely on expliit data assoiation and exploits multiple modalities suh as raw range and image data streams. Color, depth, and normal information are handled in an uniform manner and the registration is obtained by minimizing the pixel-wise differene between two multi-hannel images. We developed a flexible and general framework and implemented our approah inside that framework. We also released our implementation as open soure C++ ode. The experiments show that our approah allows for an aurate registration of the sensor data without requiring an expliit data assoiation or model-speifi adaptations to datasets or sensors. Our approah exploits the different ues in a natural and onsistent way and the registration an be done at framerate for a typial range or imaging sensor. I. INTRODUCTION Most mobile robots need to estimate a map of their surroundings in order to navigate. Thus, the task of registering the inoming sensor data suh as images or point louds is an important building blok for most autonomous systems. This funtionality is also of key importane for estimating the relative motion of a robot through inremental mathing, often alled visual odometry or laser-based odometry, depending on the used sensing modality. We investigate the problem of registering data from typial roboti sensors suh as the Kinet amera, a 3D LIDAR suh as a Velodyne laser sanner, or similar in a general way without requiring speial, sensor-speifi adaptations. More onretely our goal is to provide a general methodology to find the transformation that maximizes the overlap between two measurements taken from the same sene. To this extent, ICP is a popular strategy for registering point louds. It proeeds by iteratively alternating two steps: data assoiation and transform estimation. Data assoiation omputes pairs of orresponding points in the two louds, while transform estimation alulates an isometry that applied to one of the two louds minimizes the distane These two authors ontribute equally to the work. Igor and Cyrill are with the University of Bonn, Germany. Bartolomeo and Giorgio are with Sapienza University of Rome, Department of Computer, Control, and Management Engineering Antonio Ruberti, Rome, Italy. This work has partly been supported by the EC under the grant number H2020-ICT Flourish and the DFG under the grant number FOR 1505: Mapping on Demand. referene image urrent image initial error final error intensity depth normals Fig. 1: Our approah realized an effetive multi-ue registration without requiring features nor an expliit data assoiation between features or 3D points. between orresponding points. The weakness of ICP lies in the orrespondene searh as this step usually relies on heuristis and may introdue biases or gross errors. Over time, effetive variants of ICP that exploit the struture of the sene have been proposed [12], [14]. Using the struture either by relying on a point-to-plane or a plane to plane metri has been shown to improve the performane of the algorithm, espeially in indoor/strutured environments. Kerl et al. [9] reently proposed Dense Visual Odometry (DVO), an approah to register RGBD images by minimizing the photometri distane. The idea is to find the loation of a amera within a sene suh that the image aptured at that loation is as lose as possible to the measured RGBD image. By exploiting the image gradients, DVO does not

2 require expliit data assoiation, and it is able to ahieve unpreedented auray. The main shortoming of DVO is that it is restrited to the use of depth/intensity image pairs. In its original formulation DVO does not naturally inorporate additional strutural ues suh as normals or urvature. The use of these ues has been shown to substantially enlarge the basin of onvergene and redue the number of iterations needed to find a solution. The main ontribution of this paper is a general methodology to photometri sensor data registration that works on ues suh as olor, depth, and normal information in a unified way and does not require an expliit data assoiation between features, 3D points, or surfaes. The approah is inspired by DVO [9]. It does not require any feature extration, and operates diretly on the image or imagelike data obtained from a sensor suh as the Kinet or a 3D LIDAR. A key property of our approah is an easyto-extend, mathematially sound framework for registration that does not need to make an expliit data assoiation between sensor readings or the 3D model. In ontrast, it solves the registration problem as a minimization in the olor, depth, and normal data exploiting projetions of the sensor data. It an be seen as a generalization of DVO to handle arbitrary ues and multiple sensing modalities in a flexible way. An example of this registration is illustrated in Fig. 1. We provide an open soure C++ implementation at: In sum, we make the following key laims: Our approah (i) is a general methodology for photometri registration that works transparently with different sensor ues and avoids an expliit point-to-point or point-to-surfae data assoiation, (ii) an aurately register typial sensor ues suh as RGBD Kinet or LIDAR data exploiting the olor, depth, and normal information, (iii) robustly omputes the transformation between view points under realisti disturbanes of the initial guess, and (iv) an be exeuted fast enough to allow for online proessing of the sensor data without sensor-speifi optimizations. These four laims are baked up through the experimental evaluation. II. RELATED WORK There exist a large number of different registration approahes. One general way for aligning 3D point louds is the ICP algorithm, whih has often been used in the ontext of range data. Popular approahes use ICP together with point-to-point or point-to-plane orrespondenes [5] and generalized variants suh as GICP [12]. There exist approahes exploiting normal information suh as NICP [15] as well as global approahes [19] that use branh-and-bound tehnique oupled with standard ICP formulation. A popular and effetive approah is LOAM [20], [21] by Zhang and Singh that extrats distint surfae and orner features from the point loud and determine plane-to-point and line-topoint distanes to a voxel-grid representation. Traditional approahes to visual odometry trak sparse features in monoular images or stereo pair to estimate the relative orientation of the images [3], [11]. To deal with outliers in the data assoiation between feature points, most approahes use RANSAC to identify inlier and outlier points, ombined with a traking over multiple frames. Other approahes rely on a prior for the motion estimate, suh as onstant motion model. In presene of external sensors, suh as an IMU, the measurements an be filtered with the ahieved motion estimate [1], [22]. Another group of approahes exploits the depth data from RGBD streams to register sans and build dense models of the sene. KinetFusion by Newombe et al. [10], for example, largely impated the RGBD SLAM developments over the last 6 years. Similar to Newombe et al., the approah of Keller et al. [6] uses projetive data assoiation for RGBD SLAM in a dense model and relies on a surfelbased map [18] for traking. Similar approahes exploit the RGBD streams by defining signed distane fields where a diret voxel-based differene is omputed to perform the motion estimation [16], making intense use of both CPU and GPU parallelization. ICP is a frequently used approah for RGBD data and speial variants for denser depth images have been proposed [14], [15]. Reently, the team around Daniel Cremers has proposed semi-dense approahes using image data [2] to solve the visual odometry and SLAM problem as well as dense approahes for featureless visual odometry for RGBD data [7], [8], [9]. In this paper we propose a general and easy-to-implement methodology for multi-ue photometri registration of 3D point louds that an be seen as an extension of DVO. In ontrast to nearly all previous works, our method has been designed without onsidering a speifi sensor, nor a partiular ue, as we aim to apply the same exat algorithm in several ontexts. III. APPROACH Our approah seeks to register either two observations with respet to eah other or an observation against a 3D model. The sensor observations are assumed to have a 2D representation I suh as an image from a regular amera, a range image from a depth amera or a 3D LIDAR, or a similar type of observation. Suh a 2D measurement an be seen as an image, where eah pixel in the image plane ontains one or more hannels, i.e., I = {I } with the hannel index. Examples of suh hannels are light intensity, depth information, or surfae normals. We aim at registering the urrent observation to a model, for whih 3D information is available in form of a point loud. This model an be a given 3D model, or a point loud estimated from the previous observation(s). We refer to it as the model loud M = {p}. In addition to the 3D oordinates, eah point p M an also store multiple ues suh as light intensity or a surfae normal. The following subsetions desribe the key ingredients of our approah, see also Fig. 2 for an illustration. Se. III- A presents the overall error minimization formulation that uses three funtions, whih are sensor and/or ue-speifi and must be implemented by the user when adding a new

3 Fig. 2: Key ingredients of our framework and links to the orresponding subsetions. ue or a different sensor, everything else is handled by our framework. These funtions are: Se. III-B: Cue-speifi mapping funtion map () that desribes if and how a ue is transformed through a oordinate transformation. Se. III-C: Projetion model proj() of the sensor, e.g., the pinhole amera model Se. III-E: The orresponding Jaobians A. Photometri Error Minimization As in photometri error minimization approahes, our method seeks to iteratively minimize the pixel-wise differene between the urrent image I and the predited image I (M, ). Here, I (M, ) is a multi-hannel image obtained by projeting the model M onto a virtual amera loated at a pose P, where is a transformation matrix that transforms the points of M from the global into the loal amera oordinate system. More formally, our method seeks to minimize (1) = argmin k I u,v (M, ) Iu,v k2ω {z } u,v, eu,v (M,) = argmin u,v, > eu,v (M, ),p Mvis Eah point-wise error term ep ()> is the differene between a predited and a measured hannel evaluated at the pixel where the point p projets onto given the model of the sensor. We expand the point-wise error as follows: ep () = map (, p) Iproj( p). T = argmin wp kmap (, p) Iproj( p) k2ω, (5) p Mvis where the regularization weight wp dereases with the magnitude of the hannel errors ep () and is used to rejet outliers. The minimization is performed using a loal perturbation: x = ( tx, ty, tz, αx, αy, αz )>, {z } {z } t Ω eu,v (M, ) (2) where eu,v (M, ) denotes an error at a pixel loa T tion (u v) between the predited value I u,v (M, ) and the measured one Iu,v for a partiular hannel index. The matrix Ω = diag({ω }) is a blok diagonal information matrix used to weight the different hannels of the image. Let Mvis be the subset of points from the model M that are visible from the image plane of image I. In our urrent implementation, we onstrut this set using the depth buffer as explained in details in Se. III-D. Given Mvis, we an rewrite the sum in Eq. (1) using the points: 2 = argmin ep ()> Ω. (3) omputes the value of hannel I (M, ) evaluated at the pixel proj( p). During a first read, that may sound onfusing as the default ue light intensity is not affeted by the transformation and are simply opied from the information in the point p. Other ues, however, suh as normals or depth values are viewpoint-dependent and therefore hange depending on the given amera pose P. Our approah minimizes Eq. (3) by using a regularized least squares optimization proedure. Combining Eq. (3) with Eq. (4) and adding a per-point regularization weight wp, we an rewrite Eq. (1) in terms of points as (6) α onsisting of a translation vetor t and three Euler angles α. The vetor x is a minimal representation for the transformation matrix and Eq. (3) an be redued to a quadrati problem in x by omputing the Taylor expansion of Eq. (4) around a null perturbation as follows: ep ( x) ' = ep () + ep ( x) x ep () + Jp () x {z } {z } e p x = e p (7) + J p x (8) J p The operator is the transform omposition defined as R t R t x =, (9) {z } {z } x with R obtained by haining rotations along the three axes: R = Rx ( αx )Ry ( αy )Rz ( αz ). (10) (4) The term proj( p) = (u v) is a funtion that omputes the image oordinates obtained by projeting the point p onto a amera loated at P. The funtion map (, p) Thus, the quadrati approximation of Eq. (5) beomes x = argmin wp ke p + J p xk2ω, (11) x p Mvis C

4 and x an be found by solving a linear system of the form H x = b with the term H and b given by H = J p Ω J p (12) b = w p p M vis C w p p M vis C J p Ω ĕ p. (13) To limit the magnitude of the perturbation between iterations and thus enforing a smoother onvergene, we solve a damped linear system of the form (H + λi) x = b. (14) Solving Eq. (14) yields a perturbation x that minimizes the quadrati problem. A new solution is found by applying x to the previous solution as = x. (15) This subsetion provided the overall minimization approah and in the remainder of this setion, we desribe the parts of our approah in detail. These details are the uespeifi mapping funtion and the projetion model. Furthermore, we explain how to use the depth buffer to onstrut M vis and finally disuss the struture of the Jaobians and a pyramidal approah to the optimization. B. Cue Mapping Funtion In ontrast to traditional ICP approahes but similar to DVO [9], our method does not use an expliit point-topoint data assoiation proedure. In addition to that, by abstrating the ues into hannels of the image, our method an be extended to deal with an arbitrary number of ues and an benefit from all the available information. In our urrent implementation, we onsider the following ues: intensity, depth, range and normals. Note that additional ues or further sensor information an be added easily without hanging the framework. In this subsetion, we present the mapping funtions map () for the used ues intensity, depth, range, and normals that depend on. We will use R and t as defined in Eq. (9) as the rotation matrix and translation vetor of. Intensity: The intensity is not a geometri property of the point and thus it is not affeted by the transformation defined through, therefore the intensity value of a point intensity(p) is onsidered invariant under the map funtion, i.e., map intensity (, p) = intensity(p). (16) Depth: The depth ue of a point is the z-omponent of the point transformed by, i.e., map depth (, p) = [ ] (Rp + t). (17) Range: The range ue of a point is the norm of the point transformed by, i.e., map range (, p) = Rp + t. (18) Normals: The normal ue of a point p is the normal vetor speified by n(p) at the point rotated by, i.e., map normal (, p) = R n(p). (19) C. Projetion Models The projetion funtion maps a 3D point from the model loud onto a oordinate in a 2D image. In our implementation, we provide two projetive models, the pinhole and the spherial model. The pinhole model better aptures the harateristis of imaging sensors suh as RGBD ameras while the spherial model is entailed to 3D LIDARs. In the paper, we desribe these two models but note that the framework easily extends to other types of projetion funtions suh as the ylindrial model. Only a single funtion needs to be overridden. Pinhole Model: Let K be the amera matrix. Then, the pinhole projetion of a point p is omputed as proj pinhole (p) = π(k p) (20) f x 0 x K = 0 f y y (21) π(v) = 1 [ ] vx, (22) v z v y with the intrinsi amera parameters for the foal length f x, f y and the priniple point x, y. The funtion π(v) is the homogeneous normalization. Spherial Model: Let K be a amera matrix in the form of Eq. (21), where f x and f y speify respetively the resolution of azimuth and elevation and x and y their offset in pixels. Then, the spherial projetion of a point is given by atan2(p ( y, p x ) proj spherial (p) = K atan2 p z, 1 p 2 x + p 2 y ) (23) D. Computing Visible Points using Depth and Range Buffers As stated in the beginning of Se. III, Eq. (1) and Eq. (3) are equivalent if there are no olusions or self-olusions. This ondition an be satisfied by removing all points from the model M that would be oluded after applying the projetion. At eah iteration, the estimated position of the loud with respet to the sensor hanges, thus before omputing the projetion, we need to transform the model aording to. Subsequently, we projet eah transformed point onto an image and for eah pixel in the image, we preserve only the point that is the losest one to the observer aording to the hosen projetive model. The outome of the overall proedure is a subset of the transformed points in the model that are visible from the origin. The redued set of non-oluded points an be effetively omputed as follows. Let D be a 2D array of the size of the image, eah ell (u v) T of D ontains a depth or range value referred to as D(u, v) and a model point ˆp that generated this depth or range reading. First, all depths D(u, v) are initialized with. Then, we iterate over all points p M and perform the following omputations: Let p = Rp + t be the transformed point and let (u, v) = proj(p ) be the image oordinates of the point after the projetion.

5 If using the pinhole projetive model, let d = p z be the z omponent of the transformed point. If using the spherial model, let d = p be its norm. We ompare the range value D(u, v) previously stored in D with the range omputed for the urrent point d. If the latter is smaller than the former, we replae D(u, v) with d and ˆp with p. At the end of the proedure, D ontains all points of the model visible from the origin, i.e. are not oluded. These points form the M vis loud. E. Struture of the Jaobian In this setion, we highlight a modular struture of the Jaobian J p(), whih is key for an effiient omputation. By applying the hain rule to the right summand of Eq. (7), we obtain the following form for the Jaobian J p(): J p () = J map {}}{ map ( x, p) x (24) I u,v proj( p) ( x) p u, v u,v=proj(p) p p=p x }{{}}{{}}{{} J J J img proj tf Thus, the Jaobian an be ompatly written as: J p() = J map J img J proj J tf (25) Note that the multipliative nature of Eq. (25) in this formulation allows us to easily ompute the overall Jaobian from its individual omponents J map, J img, J proj and J tf. In partiular, J tf does not depend on the hannel, nor on the projetive model. Similarly, J proj depends only on the projetive model. The image Jaobian J img an be omputed diretly from the image through a onvolution for obtaining image gradients. Note that to inrease the preision, we reommend to ompute J img with subpixel preision through bilinear interpolation during the optimization. Only the Jaobian J img of the funtion map( ) depends on the speifi ue. For ompleteness, we report all the Jaobians for all projetive models and all hannels in Appendix I. F. Hierarhial Approah A entral hallenge for photometri minimization approahes is the hoie of the resolution in order to optimize the trade-off between the size of the onvergene basin and the auray of the solution. A high image resolution has a positive effet on the auray of the solution given the initial guess lies in the onvergene basin. This is due to the fat that more measurements are taken into aount and the preision of the typial sensor is exploited in a better way. A high resolution, however, often omes at the ost of reduing the onvergene basin sine the iterative minimization an get stuk more easily in a loal minimum arising from a high frequeny spetral omponent of the image. Thus, using lower resolution, the photometri approah exhibits an inreased onvergene basin at the ost of a lower preision. In our implementation, we leverage on these onsiderations by using a pyramidal approah. The optimization is performed at different resolutions, starting from low to high resolutions. After onvergene on one level, the optimization swithes to the next level by inreasing the resolution. The optimization on the higher resolution uses the solution from the lower level as the initial guess. Our termination riterion for the optimization on eah level of this resolution pyramid analyzes the evolution of the value of the objetive funtion in Eq. (3), normalized by the number of inliers. We stop the iterations at one level if this value does not derease between two subsequent iterations or if a maximum number of iterations is reahed. G. Brief Summary As an be seen from the overall Se. III, we provide a general methodology that builds upon photometri registration and works flexibly with different sensor ues. All that is needed for integrating a new sensor ue is an implementation of the mapping funtion map (), the projetion funtion proj(), and the Jaobians (see Appendix). Furthermore, our method does not require any expliit point-to-point or point-to-surfae orrespondene as the optimization performs the error minimization exploiting the projetions. Thus, the first of our four laims made in the introdution is baked up through Se. III. IV. EPERIMENTAL EVALUATION Our method is a general and effiient framework for multiue sensor data registration. We release a C++ implementation that losely follows the desription in this paper as open soure. We implemented our general methodology for the RGBD Kinet sensor and 3D laser sanners using the following ues: intensity, depth, range, and surfae normal and thus the evaluation is done based on these sensors and ues. Note, that further ues or similar sensors an be added easily. The evaluation presented here is designed to support the remaining three laims made in the introdution. To simplify omparisons, we onduted our experiments on publily available datasets: TUM benhmark suite [17], aquired with RGBD sensors in offie-like environments. S. Gennaro Cataomb dataset [13], reorded with a RobotEye 3D LIDAR in a ataomb environment. KITTI dataset [4], where we used the Velodyne HDL- 64E data reorded in large sale environments. Furthermore, we provide omparisons to state-of-the art approahes suh as DVO or NICP for eah dataset. The outomes of these omparisons highlight that our method, although being general and relatively easy to implement, yields to an auray that is omparable to those ahieved by systems dediated to speifi setups. A. Registration Performane and Comparison This setion is designed to show that our method an aurately register typial sensor ues suh as RGBD Kinet or LIDAR data exploiting the olor, depth, and normal information and that it an do so under realisti disturbanes

6 TABLE I: Relative Pose Error on TUM desk sequenes. DVO (as reported in paper) DVO (implementation) Our approah DVO (without intensity ue) Our approah (without intensity ue) fr1/desk2 [m/s] [deg/s] of the initial guess. The first experiment is designed to evaluate the auray of our approah. To do that, we rely on RGBD data and provide a omparison to Dense Visual Odometry (DVO) [9], whih is the urrent state-of-the-art method and the one most losely related to this paper. We used the three available hannels, namely the intensity, the depth and the normals. We performed the omparison on the four desk sequenes also used in [9] and report the relative pose error (RPE). For DVO, we used the author s open soure implementation1. For ompleteness, we report in Tab. I both, the results presented in the paper [9] and the ones we obtained with the open soure implementation. All values have been omputed using the evaluation sript provided with the TUM benhmark suite. Our approah provides slightly lower but overall omparable performane than DVO yielding a low relative error both, in terms of translation and rotation, respetively in the order of 10 2 m/s and 1 deg/s, see Tab. I. We onduted a seond experiment with the TUM dataset, where we removed the intensity hannel, using only the depth images and the normals derived from the depth image to perform the registration. By exploiting the normal ue, our approah provides results onsistent to the ones obtained when using also the intensity, see last two rows on Tab. I. In ontrast to that, DVO was unable to perform the registration without the intensity hannel. This is oherent with the operating onditions whih DVO was designed for and at the same time supports the general appliability of our method to different ues. The third experiment aims at showing the effetiveness of our algorithm when dealing with dense 3D laser data. We used a dataset aquired in the S. Gennaro ataomb of Naples within the EU projet ROVINA. The data was reorded with a Oular RobotEye RE05 3D laser sanner using a maximum range of 30 m. Sine the dataset does not provide ground truth, we performed a qualitative omparison with Normal ICP (NICP) by Serafin et al. [15]. The 3D point louds have been reorded in a stop and go fashion at an average distane of about 1.7 m between two onseutive sans. The robot has traks and this provides a rather poor odometry information. This odometry is used as the initial guess for the optimization and the same guess was also used for NICP, that is used for omparisons in this experiment. To perform the registration we use both, range hannel and the normals hannel omputed on the range image. There is no intensity information available for this dataset. 1 fr1/desk [m/s] [deg/s] fr2/desk [m/s] [deg/s] fr2/person [m/s] [deg/s] Our Approah NICP Inonsisteny Fig. 3: S. Gennaro ataombs dataset, reorded with a RobotEye 3D LIDAR. Diret omparison of our approah with Normal ICP (NICP). The latter shows a registration inonsisteny in the middle of the trajetory, with an unexpeted hange of the orientation around the vertial axis. range normals referene image urrent image initial error χ2 = 5904 final error χ2 = 32.4 Fig. 4: Illustration of the error redution while registering two images of the S. Gennaro dataset (best viewed on sreen). Kitti Sequene GT Our Approah z [m] Approah / setup x [m] Fig. 5: Ground truth omparison in the sequene 10 of the KITTI dataset. range normals ref.image urr.image init error final error Fig. 6: Illustration of the error before and after registering two images of the KITTI dataset. The images are obtained by projeting the Velodyne HDL-64 louds using a spherial projetor (best viewed on sreen).

7 Fig. 3 illustrates the two reonstrutions of one of the sequenes of the S. Gennaro ataombs, while Fig. 4 shows the error of a pairwise registration before and after the alignment. Thanks to the abstration provided by the map funtion and the projetive model (see Se. III-B and III-C), our method an deal with both RGBD data and 3D sans in a uniform manner. To further stress the generality of our method, we onduted an additional experiment using exatly the same ode and parameters using the sequene 10 of the KITTI dataset [4] where we used the 3D sans obtained with a Velodyne HDL-64E LIDAR. As in the previous experiment, the range and normals ues are used. Fig. 6 illustrates the error redution after the registration of two sans. As shown in Fig. 5, our output reflets the ground truth with an error of the 7% of the trajetory length, with a rotational error of deg/m. Albeit reasonable, this auray is still below the one provided by approahes dediated to the sparse LIDAR suh as LOAM [21]. We see the reason of this lower performane in the quantization effets affeting the projetions when the louds are sparse. We plan to address this aspet in future versions by using anisotropi projetion funtions. Moreover, approahes suh as LOAM take advantage from edge and planar surfae features, whose usage partiularly helps in ases of struture lak. B. Runtime The next set of experiments is designed to support the last laim, namely that our approah an be exeuted fast enough to allow for online proessing of the sensor data without sensor-speifi optimizations. Thus, we report in the remainder of this setion the runtime statistis. We performed all the presented experiments on two different omputers running Ubuntu One is a laptop equipped with a i7-3630mq CPU with 2.40 GHz and the seond one is a desktop omputer equipped with a i7-7700k CPU with 4.20 GHz. Our software runs on a single ore and in a single thread. Tab. II summarizes the runtime results for the different onfigurations presented above. We provide the average results obtained in the four TUM desk sequenes for the Kinet RGBD and depth-only onfigurations, as well as for the two LIDAR setups. As listed in the table, our method an be exeuted fast and in an online fashion. On a mobile i7 CPU, we ahieve average frame rates of 20 Hz with Kinet RGBD sensor and 14 Hz with the Velodyne HDL-64E data, while we ahieve average of 30 Hz and 23 Hz on an i7 desktop omputer for the same onfigurations. Furthermore note, that our approah has a small memory footprint. For the whole registration proedure of the three datasets, we always required less than 200 MB (Kinet: 178 MB; RobotEye: 140 MB; HDL-64E: 198 MB). Note that reording a single RobotEye point louds takes around 30 s, i.e. the robot stops, reords, and then restarts. Thus, this setup may not be onsidered for real-time usage. In ontrast, the Velodyne louds of the KITTI dataset have TABLE II: Average image proessing runtime with std. deviation Sensor laptop omputer desktop omputer i7-3630mq 2.4 GHz i7-7700k 4.2 GHz Kinet 49 ms ± 0.8 ms 20.1 Hz 33 ms ± 0.6 ms 29.8 Hz RobotEye 324 ms ± 1.5 ms 3.1 Hz 250 ms ± 0.8 ms 4 Hz HDL-64E 67 ms ± 0.3 ms 14.8 Hz 43 ms ± 0.1 ms 23.1 Hz been reorded at an average frame rate of 10 Hz and the data an be proessed online. In summary, our evaluation suggests that our method provides ompetitive results in several different senarios ompared to approahes dediated to a speifi setup. At the same time, our method is fast enough for online proessing and has small memory demands, proportional to the number of hannels in use. Thus, we onlude that we supported all our four laims made in the introdution. V. CONCLUSION In this paper, we presented a general framework for registering sensor data suh as RGBD or 3D LIDAR data. Our approah extends dense visual odometry and operates on the different available ues suh as olor image, depth, and normal information. Our method avoids an expliit data assoiation and operates by diret error minimization using projetions of the sensor data or model. This allows us to suessfully register data effetively without triky, sensorspeifi adaptations. We implemented and evaluated our approah on different datasets and provided omparisons to other existing tehniques and supported all laims made in this paper. The experiments suggest that we an aurately register RGBD and 3D data under realisti onfigurations and that the omputations an be exeuted at the sensor framerate on a regular notebook omputer using a single ore. APPENDI I JACOBIANS In this setion, we fous in more detail on eah part of the Jaobian in Eq. (25) and speify all terms used of this equation. A. Jaobian of Transformation We are using the v2t( ) operator to denote the onversion between and x as defined in Eq. (6). By applying the v2t( ) operator, we obtain the standard Jaobian: J tf = v2t(x) p x = [ ] I p (26) where I is a 3 3 identity matrix and p denotes the skewsymmetri matrix formed from p. B. Map Funtion Jaobian The map funtion Jaobian J map differs for eah of the onsidered hannels. In this work, we use the following hannels: intensity, depth, range and normals. In the following we present the Jaobian derivation of eah of these ues. Intensity: The map funtion does not affet the intensity, thus we have: map intensity ( x, p) x = 0 (27)

8 Depth: We have already omputed the derivative of the transformation funtion J tf in Eq. (26). For RGBD sensors, the depth is omputed as the z oordinate of the transformed point p. Thus, the map Jaobian for the depth hannel is the third row of J tf map depth ( x, p) x = [ ] J tf (28) Range: When using a 3D LIDAR, the range r = p replaes the depth. Thus, the Jaobian J range map is omputed as J range map = maprange ( x, p) x range(v2t(x) p) = x = range( p) v2t(x) p p p=p x = range( p) J tf p p=p 1 [ ] = px p y p z Jtf (29) norm(p) Normals: The mapping funtion applied to the normals ue depends on the rotational part of and the Jaobian is given by map normal ( x, p) x = [ ] 0 [R n(p)], (30) where R denotes a rotation matrix, n(p) the normal defined for the point p, and [R n(p)] the skew-symmetri matrix. C. Image Jaobian The Image Jaobian J img is numerially omputed for eah hannel with pixel-wise derivation: I u,v = 1 ( I u 2 u+1,v I u 1,v) I u,v = 1 ( I v 2 u,v+1 I u,v 1) D. Jaobian of Projetion (31) The Jaobian of the projetion depends diretly on the projetion funtion J proj = proj(p) p. In the following, we provide details for the two models given in Se. III-C. Pinhole Model: We derive the projetion Jaobian for a pinhole amera from Eq. (22): proj(p) = π(p ) K p p p (32) p =K p p = 1 [ z 0 x z 2 K 0 z y] (x,y,z)=k p Spherial Model: The projetion funtion for the range sensors is defined in Eq. (23). We make use of the substitution a 2 = p 2 x + p 2 y, and define the Jaobian as follows: [ py p x 0 ] proj(p) p = 1 a 2 2 +p2 z 1 a 2 2 [ p x p z ] py pz a 2 a 2 a 2 K (33) [ ] REFERENCES [1] M. Bloesh, S. Omari, M. Hutter, and R. Siegwart. Robust visual inertial odometry using a diret ekf-based approah. In Pro. of the IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), pages , Sept [2] J. Engel, T. Shöps, and D. Cremers. Lsd-slam: Large-sale diret monoular slam. In Pro. of the Europ. Conf. on Computer Vision (ECCV), pages Springer, Cham, [3] C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Saramuzza. Svo: Semidiret visual odometry for monoular and multiamera systems. IEEE Trans. on Robotis (TRO), 33(2): , April [4] A. Geiger, P. Lenz, and R. Urtasun. Are we ready for autonomous driving? the kitti vision benhmark suite. In Pro. of the IEEE Conf. on Computer Vision and Pattern Reognition (CVPR), [5] M. Jaimez and J. Gonzalez-Jimenez. Fast visual odometry for 3-d range sensors. IEEE Trans. on Robotis (TRO), 31(4): , Aug [6] M. Keller, D. Lefloh, M. Lambers, and S. Izadi. Real-time 3D Reonstrution in Dynami Senes using Point-based Fusion. In Pro. of the International Conferene on 3D Vision (3DV), pages 1 8, [7] C. Kerl, J. Stukler, and D. Cremers. Dense ontinuous-time traking and mapping with rolling shutter rgb-d ameras. In Pro. of the IEEE Intl. Conf. on Computer Vision (ICCV), pages , [8] C. Kerl, J. Sturm, and D. Cremers. Dense visual slam for rgb-d ameras. In Pro. of the IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), pages IEEE, [9] C. Kerl, J. Sturm, and D. Cremers. Robust odometry estimation for rgb-d ameras. In Pro. of the IEEE Intl. Conf. on Robotis & Automation (ICRA), pages IEEE, [10] R. A. Newombe, S. Izadi, O. Hilliges, D. Molyneaux, D. Kim, A. J. Davison, P. Kohli, J. Shotton, S. Hodges, and A. Fitzgibbon. Kinet- Fusion: Real-Time Dense Surfae Mapping and Traking. In Pro. of the Intl. Symposium on Mixed and Augmented Reality (ISMAR), pages , [11] D. Nister, O. Naroditsky, and J. Bergen. Visual odometry. In Pro. of the IEEE Conf. on Computer Vision and Pattern Reognition (CVPR), volume 1, pages I 652 I 659 Vol.1, June [12] A. Segal, D. Haehnel, and S. Thrun. Generalized-ip. In Pro. of Robotis: Siene and Systems (RSS), volume 2, [13] J. Serafin, M. Di Cio, T. M. Bonanni, G. Grisetti, L. Iohi, D. Nardi, C. Stahniss, and V. A. Ziparo. Robots for exploration, digital preservation and visualization of arheologial sites. In Artifiial Intelligene for Cultural Heritage, hapter 5, pages [14] J. Serafin and G. Grisetti. NICP: Dense Normal Based Point Cloud Registration. In Pro. of the IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), pages , [15] J. Serafin and G. Grisetti. Using extended measurements and sene merging for effiient and robust point loud registration. Journal on Robotis and Autonomous Systems (RAS), 92(C):91 106, June [16] M. Slavheva, W. Kehl, N. Navab, and S. Ili. Sdf-2-sdf: Highly aurate 3d objet reonstrution. In Pro. of the Europ. Conf. on Computer Vision (ECCV), pages Springer International Publishing, [17] J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers. A benhmark for the evaluation of rgb-d slam systems. In Pro. of the IEEE/RSJ Intl. Conf. on Intelligent Robots and Systems (IROS), Ot [18] T. Weise, T. Wismer, B. Leibe, and L. Van Gool. Online loop losure for real-time interative 3D sanning. Journal of Computer Vision and Image Understanding (CVIU), 115: , [19] J. Yang, H. Li, D. Campbell, and Y. Jia. Go-ip: A globally optimal solution to 3d ip point-set registration. IEEE Trans. on Pattern Analalysis and Mahine Intelligene (TPAMI), 38(11): , Nov [20] J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. In Pro. of Robotis: Siene and Systems (RSS), [21] J. Zhang and S. Singh. Low-drift and real-time lidar odometry and mapping. Autonomous Robots, pages 1 16, [22]. Zheng, Z. Moratto, M. Li, and A. Mourikis. Photometri Path- Based Visual-Inertial Odometry. In Pro. of the IEEE Intl. Conf. on Robotis & Automation (ICRA), 2017.