Real-Time Camera Tracking and 3D Reconstruction Using Signed Distance Functions

Transcription

1 Real-Tie Caera Tracking and 3D Reconstruction Using Signed Distance Functions Erik Bylow, Jürgen Stur, Christian Kerl, Fredrik Kahl and Daniel Creers Center for Matheatical Sciences, Lund University, Lund, Sweden Eail: Departent of Coputer Science, Technical University of Munich, Garching, Gerany Eail: Abstract The ability to quickly acquire 3D odels is an essential capability needed in any disciplines including robotics, coputer vision, geodesy, and architecture. In this paper we present a novel ethod for real-tie caera tracking and 3D reconstruction of static indoor environents using an RGB-D sensor. We show that by representing the geoetry with a signed distance function (SDF), the caera pose can be efficiently estiated by directly iniizing the error of the depth iages on the SDF. As the SDF contains the distances to the surface for each voxel, the pose optiization can be carried out extreely fast. By iteratively estiating the caera poses and integrating the RGB-D data in the voxel grid, a detailed reconstruction of an indoor environent can be achieved. We present reconstructions of several roos using a hand-held sensor and fro onboard an autonoous quadrocopter. Our extensive evaluation on publicly available benchark data shows that our approach is ore accurate and robust than the iterated closest point algorith (ICP) used by KinectFusion, and yields often a coparable accuracy at uch higher speed to feature-based bundle adjustent ethods such as RGB-D SLAM for up to ediu-sized scenes. I. INTRODUCTION 3D siultaneous localization and apping (SLAM) is a highly active research area as it is a pre-requisite for any robotic tasks such as localization, navigation, exploration, and path planning. To be truly useful, such systes require the fast and accurate estiation of the robot pose and the scene geoetry. The straightforward acquisition of 3D odels is also of interest for augented reality applications including coputer gaes, hoe decoration, and refurbishent easures. For exaple, designers and architectures can benefit fro this technology as it allows the to cost-effectively scan a roo or a piece of furniture before starting the actual work. Two exaples of 3D odels acquired with our approach are shown in Figure 1. Our scanning equipent consists of a handheld Asus Xtion Pro Live sensor and a laptop with a Quadro GPU fro Nvidia. The laptop provides a live view of the reconstructed odel. Both odels were acquired in realtie fro 1000 iages (approx. 30 seconds). As can be seen in the figure, the resulting odels are highly detailed and provide absolute etric inforation about the scene which is useful for a large variety of subsequent tasks. This work has partially been supported by the DFG under contract nuber FO 180/17-1 in the Mapping on Deand (MOD) project. Fig. 1: Reconstruction of a roo with a handheld sensor using our approach. Caera tracking and reconstruction runs in real-tie on a laptop and provides live feedback to the user. Top row: living roo. Botto row: study roo. Structure fro otion (SfM) techniques fro coputer vision typically use iages fro a oving caera. By extracting and atching visual features across several views, bundle adjustent can be used to estiate both the caera poses and a sparse 3D odel of the feature points [13, 2]. Furtherore, several ethods have been proposed to copute dense depth aps fro iage data [10, 21]. The advent of depth sensors like the Microsoft Kinect opened new possibilities for approaches on dense 3D reconstruction, as they directly output dense depth iages. Accordingly, we investigate in this paper how dense depth iages can be efficiently registered and integrated, and how the scene geoetry and texture can be represented to facilitate this endeavor. Recently, Newcobe et al. [18] have presented ipressive results by using signed distance functions (SDFs) to represent the scene geoetry and the iterated closest point (ICP) algorith for caera tracking. Whelan et al. [24] extended this approach with a rolling reconstruction volue and color fusion, and evaluated alternative ethods for visual odoetry estiation. However, pure visual odoetry is inherently subject to significant drift, so that the registration with respect to a world odel is in our point of view preferable. The contribution of this work is a novel ethod for esti-

2 ating the caera otion directly based on the SDF. The key insight behind our approach is that the SDF already encodes the distance of each voxel to the surface. As a result, we do not need explicit data association or downsapling as in ICP to achieve real-tie perforance. We present an extensive evaluation of our approach on public benchark datasets. In coparison to previous ethods, we found that our approach yields ore accurate and ore robust tracking than ICP-based KinectFusion (as ipleented in the point cloud library [1]) and it is often coparable to the featurebased RGB-D SLAM [8]. Furtherore, we deonstrate that our approach is stable enough to use it for position control of an autonoous quadrocopter. II. RELATED WORK Siultaneous localization and apping refers to both the estiation of the caera pose and apping of the environent. This requires a suitable representation of the scene geoetry, and the choice of this representation strongly influences the efficiency of pose estiation and ap optiization. Laser-based localization and apping approaches often use scan atching or the iterated closest point algorith (ICP) [4] to estiate the otion between fraes. Graph SLAM ethods use these otion estiates as input to construct and optiize a pose graph [15]. Typically, these ethods render a joint ap only after pose graph optiization, and this ap is generally not used for further pose optiization. The resulting aps are often represented as occupancy grid aps or octrees [25] and are therefore well suited for robot localization or path planning. Henry et al. [9] were the first to apply the Graph SLAM approach to RGB-D data using a cobination of visual features and ICP. A siilar syste was recently presented by Endres et al. [8] and extensively evaluated on a public benchark [22]. In this paper, we copare the perforance of our approach to the RGB-D SLAM syste and deonstrate that we often achieve a coparable tracking perforance at a higher frae-rate. Newcobe et al. [18] recently deonstrated with their faous KinectFusion approach that dense reconstruction is possible in real-tie by using a Microsoft Kinect sensor. To represent the geoetry, Newcobe et al. eploy a signed distance function (SDF) [7] and use ICP in a coarse-tofine anner to estiate the caera otion. For each iage, the algorith first renders a point cloud fro the SDF at the previous pose using ray tracing and subsequently aligns this with the next depth iage. Point correspondences are found using projective data association [5] and the point-to-plane distance. As the original ipleentation is not available and no benchark evaluation is provided, we copare our approach to the KinFu open-source ipleentation as available in the point cloud library [1]. We show in this paper that our approach outperfors KinFu in ters of speed and accuracy. While ICP only iniizes the error on point clouds, several approaches have recently appeared that iniize the photoetric error [20, 12] or cobinations of both [23], however without subsequent 3D reconstruction. Whelan et al. [24] recently integrated these ethods with the KinectFusion approach and deonstrated that superior tracking perforance can be achieved, however without evaluating the global consistency of the resulting odel. Canelhas [6] developed in his aster s thesis an approach for caera tracking siilar to ours. However, his focus lies ore on object detection and recognition in an SDF, and no thorough evaluation of the accuracy was perfored. Kubacki et al. [14] showed how an SDF can be used to estiate the caera pose, however, only on synthetic data and without a coparative evaluation. Recently, Ren and Reid [19] deonstrated SDF-based object tracking where they assue a known object odel. Based on this, they generate a SDF that they subsequently use for object tracking. By using a robust cost function, robustness to fast caera otions and partial occlusions can be achieved. In this paper, we describe (1) a direct approach to caera tracking on SDFs, (2) present a thorough evaluation of SDFbased tracking and apping on public bencharks, and (3) copare the tracking perforance to existing real-tie solutions. We study the influence of alternative distance etrics, weighting functions and different caera otions on a large nuber of different scenes. Furtherore, we deonstrate that our approach is directly applicable to position control of a quadrocopter and the autoatic 3D reconstruction of roos. III. NOTATION AND PRELIMINARIES We denote a 3D point as x R 3. Further, we denote the rotation of the caera as R SO(3) and the translation as t R 3. At each tie step, an RGB-D caera outputs a color and a depth iage, to which we refer to by the functions I RGB : R 2 R 3 and I d : R 2 R. (1) We assue that the depth iage is already registered to the color iage, so that pixels correspond one-to-one. We assue the pinhole caera odel with intrinsic paraeters f x, f y, c x and c y corresponding to the focal length and the optical center. According to this odel, a 3D point x = (x, y, z) is projected onto the iage plane by ( fx x π(x, y, z) = z + c x, f yy z + c y ) (2) and we can reconstruct the 3D point corresponding to a pixel (i, j) R 2 with depth z = I d (i, j) by ( (i cx )z ρ(i, j, z) =, (j c ) y)z, z. (3) f x f y IV. APPROACH We represent the geoetry using a signed distance function stored in a voxel grid. We follow an iterative approach where we first estiate the caera pose given the previous SDF, and then update the SDF based on the newly coputed caera pose. Note that we optiize the caera pose directly on the SDF, while KinectFusion [18] first generates a synthetic depth iages that it subsequently aligns to the current depth iage using ICP.

3 A. Caera Tracking In this part, we present how we estiate the caera otion given an SDF and a depth iage. For now, we assue that we already have a representation of the geoetry acquired fro the previous depth iage given by the SDF ψ : R 3 R. (4) This function returns for any point x R 3 the signed distance fro x to the surface. The idea is now to use the SDF to construct an error etric that describes how well a depth iage I d fits to the SDF. For each pixel (i, j), we have its depth z = I d (i, j). Given this, we can reconstruct the corresponding 3D point x ij in the local coordinate syste of the caera by (3). We can transfor this point to the global coordinate frae using x G ij = Rx ij + t, (5) and now query the SDF to read out its distance fro the surface. Given that the SDF and the caera pose is correct, the reported value should then be zero. We seek to find the caera rotation R and translation t such that all reprojected points fro the depth iage lie as close as possible to the zero-crossing in the SDF (=surface), i.e., {x ψ(x) = 0}. (6) This idea is illustrated in Figure 2. By assuing Gaussian noise in the depth easureents of the caera and that all pixels are independent and identically distributed, the likelihood of observing a depth iage I d fro caera pose R, t becoes p(i d R, t) i,j exp( ψ(rx ij + t) 2 ). (7) Our goal is now to find the caera pose R, t that axiizes this likelihood, i.e., (R, t ) = arg ax R,t p(i d R, t). (8) To siplify subsequent coputations, we take the negative logarith and define the error function E(R, t) = i,j ψ(rx ij + t) 2, (9) where i, j iterate over all pixels in the depth iage (in our case ). Reeber that in an SDF, all points on the surface have a distance of zero. Therefore, in the noise free case, our error function would return zero as all points would exactly reproject onto the surface. In practice, due to noise, the error function will never be exactly zero. Moreover, not all pixels in the depth iage are defined (for exaple, due to issing values because of occlusions or poor reflectance). Furtherore, the voxel grid underlying the SDF has a finite size and ay also have issing values. Therefore, we count the nuber of evaluated pixels n and noralize the error accordingly. To iniize this error function we use the Lie algebra representation of rigid-body otion as described in Ma et al. Fig. 2: Our goal is to find the caera pose ξ n+1 such that the SDF values between the reprojected 3D points is iniized. The SDF is constructed fro the first n depth iages and corresponding caera poses ξ 1,..., ξ n. [17]. A rigid-body otion can be described with the 6- diensional twist coordinates ξ = (ω 1, ω 2, ω 3, v 1, v 2, v 3 ), (10) where (v 1, v 2, v 3 ) refer to the translational and (ω 1, ω 2, ω 3 ) to the rotational coponents. The advantage of the Lie algebra is that it is a inial representation, i.e., it has only six degrees of freedo. Using this notation, we can rewrite (9) as where E(ξ) = i,j ψ ij (ξ) 2, (11) ψ ij (ξ) = ψ(rx ij + t). (12) Our goal is now to find the twist ξ that iniizes this function. To solve this, we apply the Gauss-Newton ethod for nonlinear iniization. We start by linearizing ψ around our initial pose estiate ξ (0) that we set to the estiated previous caera pose ξ n of tie step n, ψ(ξ) ψ(ξ (k) ) + ψ(ξ (k) ) (ξ ξ (k) ), (13) where ψ(ξ (k) ) is the derivative of the signed distance function evaluated at ξ (k) and k is the iteration step. Plugging this into (11) gives us a quadratic for that approxiates the original error function, i.e., E approx (ξ) = i,j (ψ ij (ξ (k) ) + ψ ij (ξ (k) ) (ξ ξ (k) )) 2. (14) We can efficiently iniize this equation by putting its derivative to zero, i.e., which, after soe calculations, becoes ψ ij (ξ (k) ) ψ ij (ξ (k) )+ i,j d dξ E approx(ξ) = 0, (15) ψ ij (ξ (k) ) ψ ij (ξ (k) ) (ξ ξ (k) ) = 0. (16)

4 Here ψ ij (ξ (k) ) ψ ij (ξ (k) ) gives a 6-diensional vector b ij for each pixel and ψ ij (ξ (k) ) ψ ij (ξ (k) ) gives a 6 6- diensional atrix A ij for each pixel. By defining A := i,j ψ ij (ξ (k) ) ψ ij (ξ (k) ) R 6 6, (17) b := i,j we can rewrite (16) as ψ ij (ξ (k) ) ψ ij (ξ (k) ) R 6 1, (18) b + Aξ Aξ (k) = 0. (19) Fro this, we can copute the caera pose that iniizes the linearized error as ξ (k+1) = ξ (k) A 1 b, (20) which can quickly be coputed as A is only a 6 6 atrix. Based on this new estiate, we re-linearize the original error function (11) around ξ, and solve iteratively (20) until convergence. We stop when either the change ξ (k+1) ξ (k) is sall enough or when a certain nuber of iterations is exceeded. Upon convergence, we assign the final pose estiate to the current caera ξ n+1. In order to obtain real-tie perforance, we coputed all vectors b ij and atrices A ij in parallel on the GPU since they are independent of each other. B. Representation of the SDF As detailed by Curless and Levoy [7], we represent the SDF using a discrete voxel grid of resolution. We allocate two grids in eory, where one stores the averaged distances, and the second one stores the su of all weights, i.e., D : [0,..., 1] 3 R, (21) W : [0,..., 1] 3 R. (22) Keeping track of the weight in each cell allows us to handle occlusion and sensor uncertainty appropriately, as we will detail in the next subsection. Given a reconstruction volue of diension width height depth, a world point x = (x, y, z) R 3 is apped to voxel coordinates i x/width j = y/height (23) k z/depth Since (i, j, k) is generally non-integer, we deterine the signed distance value ψ(x) by tri-linear interpolation between the values of its eight integer neighbors. C. Distance and Weighting Functions To integrate a new depth iage, we need to deterine which voxels have been observed by the depth caera and update their distances accordingly. While Curless and Levoy [7] originally proposed to perfor ray casting fro the sensor, we follow the opposite approach siilar to [18] by projecting each voxel onto the iage plane. This has the advantage that every voxel is visited exactly once, which is hard to ensure (a) Fig. 3: Visualization of the projective point-to-point distance (a) and the point-to-plane distance (b). Note that coputing the true distance is coputationally involved. in the ray casting approach. As the operation that has to be carried out for each voxel is independent of its neighbors, this process can be easily parallelized on the GPU. In contrast to previous work, we present in the following several alternative distance and weighting functions and study their influence on the perforance. To ipleent this strategy, we need to copute the distance of each voxel to the observed surface as represented by the current depth iage. Note that coputing the true distance of each voxel is tie consuing, as it requires the coputation of all shortest paths over the entire volue. Even efficient algoriths such as Fast Marching [3] are not well suited for real-tie applications. Therefore, we resort instead to an approxiation based on the projective distance. We investigated both the projective point-to-point and point-to-plane etric as illustrated in Figure 3 with ore details given in the next two subsections. 1) Projective Point-To-Point: For each vertex we have its global (center) coordinates x G. Given the pose of the current caera R, t, we can transfer these coordinates in the local coordinate frae of the caera as (b) x = (x, y, z) = R (x G t). (24) According to our caera odel (2), this point gets projected to the pixel (i, j) = π(x) (25) in the iage. We define then the projective point-to-point distance as the difference of the depth of the voxel and the observed depth at (i, j), i.e., d point-to-point (x) := z I d (i, j). (26) Note that this distance is signed, i.e., negative values are assigned to voxels in front of the observed surface, and positive values to voxels behind. 2) Projective Point-To-Plane: Our otivation for the investigating the point-to-plane etric was that the point-topoint etric gets increasingly inaccurate the less the viewing angle is orthogonal to the surface (see Figure 3 b). This can be resolved by using the point-to-plane distance under the assuption that the observed surface is locally planar. As a first step, we apply a bilateral filter to the depth iage and copute the norals for all pixels. Given a voxel x, we

5 Fig. 4: We truncate large estiated distances to liit the influence of approxiation errors and noise. copute its corresponding pixel coordinates (i, j) and read out the observed surface noral n(i, j). The point-to-plane distance can then be coputed as d point-to-plane (x) := (y x) n(i, j). (27) Again, note that this distance is signed, i.e., negative values refer to voxels in front of the surface and positive values to voxels behind. 3) Truncation and Weighting: As projective distance etrics are only approxiations of the true distance function, they can be highly erroneous, especially when the estiated distances are large. However, for tracking and reconstruction, we are in particular interested that a sall band around the zero-crossing is accurately estiated in the SDF. Therefore, we truncate the projected distances d and apply a weighting ter that blends out large distances. For truncation, we use d trunc = δ if d < δ d if d δ δ if d >δ (28) The truncation function is also illustrated in Figure 4. Note that due to this truncation, the gradient of our SDF will be zero in regions that are far away fro the estiated surface. Furtherore, we eploy a weighting function to give higher weights to voxels in front of the observed surface and lower weights to voxels behind. Depending on the observation odel of the depth sensor, different weighting functions can be used. In this work, we study six different weighting functions as depicted in Figure 5: The constant weight trivially assues a constant weight for all voxels, i.e., w const (d) = 1. (29) This is suitable for distance sensors that can (deeply) penetrate objects, such as a radar. The linear weight, as proposed by Curless and Levoy [7] and used in KinectFusion, assigns a constant weight to all voxels up to a certain penetration depth ɛ. After this depth, the weight linearly decreases to zero at penetration depth δ: w lin (d) = 1 if d < ɛ δ d δ ɛ if d ɛ and d δ. 0 if d > δ (30) The linear odel expresses a certain prior for the iniu depth δ of objects in the scene and a linear prior afterwards. In Fig. 5: We evaluated six different weighting functions for data fusion. contrast to this, we propose an exponential weighting function otivated by a Gaussian noise odel of depth easureents, i.e., w exp (d) = 1 if d < ɛ e σ(d ɛ)2 if d ɛ and d δ. 0 if d > δ (31) As we will show in our experiental evaluation, this weighting function leads to a slightly higher robustness than the linear weight function. D. Data Fusion and 3D Reconstruction Given a sequence of (approxiate) distance easureents and the weights for a particular voxel cell x = (i, j, k), our goal is to fuse all of these easureents to obtain the best possible estiate for ψ(x). As this estiation process can be carried out independently for each voxel, we drop the index x in the reainder of this subsection. For this, we follow the approach of Curless and Levoy [7], that we briefly suarize here. Under the assuption that all distance easureents are norally distributed, this estiation proble can be forulated as follows. We seek the distance ψ that axiizes the observation likelihood n p(d 1, w 1,..., d n, w n ψ) exp ( 12 ) w i(ψ d i ) 2, i=1 (32) where d i and w i refer to the observed truncated distances and weights in frae i, respectively. After taking the negative logarith, we obtain a quadratic error function L(ψ) = n i=1 1 2 w i(ψ d i ) 2 (33) that we ai to iniize. By putting the derivative of L(ψ) to zero we obtain n i=1 ψ = w id i n i=1 w, (34) i which eans that the optial ψ is the weighted average of all easureents. Therefore, we can calculate the estiated

6 where θ is the angle between the ray and the principal axis to give ore weight to pixels whose noral is pointing towards the caera. While extracting the triangle esh using arching cubes, we query the color grid to copute colors for vertices using tri-linear interpolation. V. R ESULTS start/end (a) (b) Fig. 6: On sall work spaces, our ethod is nearly drift-free. (a) 3D reconstruction and the estiated caera trajectory of a sall office scene. (b) Visualization of the (downsapled) voxel grid underlying the reconstruction volue ( = 256). signed distance of each voxel as a running weighted average, i.e., W D + wn+1 dn+1 W + wn+1 W W + wn+1. D (35) (36) Note that this coputation has to be carried our for each voxel. As each voxel does not depend on its neighbors, this update can easily be coputed in parallel for all voxels on the GPU. E. Meshing and Colorization The SDF encodes the 3D geoetry of the scene. As the surface is located at the zero crossing in the signed distance function, we apply a straight-forward ipleentation of the arching cubes algorith [16] to extract the corresponding triangle esh. Next to the estiation of the geoetry, we also estiate a color texture of the scene siilar to [24]. We represent this texture using an additional voxel grid consisting of three channels R, G, B for the color and one additional channel for the color weights Wc. The coputation can be carried out in parallel during the update step of the SDF as described in Section IV-D. Given that a voxel is sufficiently close to the surface, i.e., kdk <, we retrieve the observed color (r, g, b)> = IRGB (i, j) (37) fro the RGB iage and update the color estiate as the running average Wc R + wcn+1 r Wc + wcn+1 Wc G + wcn+1 g G Wc + wcn+1 Wc B + wcn+1 b B Wc + wcn+1 R (38) (39) (40) where wcn+1 is the weight for the new easureent. As weight, we use wcn+1 = wn+1 cos θ, (41) In this section we present both qualitative results of 3D reconstructions fro live-data and quantitative results on the TUM RGB-D benchark [22]. We copare the perforance our algorith to KinFu [1] and RGB-D SLAM [8], and study the influence of the truncation paraeter and alternative weighting functions on the tracking perforance. Finally, we analyze the robustness of our approach with respect to fast caera oveents and provide runtie easureents. A. Qualitative Results Figures 1 and 6 show several live reconstructions of roos using our algorith at a grid resolution of = 512 and = 256, respectively. The resulting reconstruction is highly detailed and etrically accurate, so that it could for exaple be used by architects and interior designers for planning and visualization tasks. We observe that our ethod is alost drift-free for sall scenes, as can be seen in Figure 6a, where we started and ended a rectangular caera otion at the sae spot. Moreover, fine details such as the cover of the RSS proceedings appear sharply. Furtherore, we used our approach for 3D reconstruction fro an autonoous quadrocopter (see Figure 7) equipped with an RGB-D caera. Note that tracking and reconstruction were carried out in real-tie on an external ground station with GPU support. The estiated pose was directly used for position control. This deonstrates that our technique is applicable for the navigation of quadrocopters and other robots. The video provided as suppleental aterial further illustrates our experiental setups and provides additional views of the reconstructed odels. B. Benchark Evaluation We also evaluated our approach on the TUM RGB-D benchark [22]. As coparison we used the KinFu ipleentation [1] and RGB-D SLAM [8]. For the ICP-step of KinFu, we explored a large range of different paraeter settings and selected the one with the highest perforance. In this evaluation, we chose δ = 0.3 and = The results are given in Table I. Our approach clearly outperfors KinFu which diverges in particular on the faster sequences. We believe that this is due to the fact that KinFu looses uch valuable inforation because of the downprojection of the SDF to a synthetic depth iage prior to caera tracking. In particular for large caera otions, the synthetic iage is taken fro a substantially different viewpoint so that the alignent process is ore difficult. For our algorith, we found that the point-to-point etric provides

7 TABLE I: The root-ean square absolute trajectory error for KinFu and our ethod for different resolutions, etrics and datasets. Also the result for RGB-D SLAM are presented. Method Res. Teddy KinFu KinFu Point-To-Plane Point-To-Plane Point-To-Point Point-To-Point RGB-D SLAM F1 Desk F1 Desk F3 Household F1 Floor F1 360 F1 Roo F1 Plant F1 RPY F1 XYZ Failed Failed Failed Teddy Desk RMSE () RMSE () Desk Household Truncation δ () Step size k (a) (b) Fig. 8: (a) The choice of the truncation paraeter δ depends on the average depth of the objects in the scene. For typical indoor scenes, δ = 0.3 is a good choice. (b) RMSE when using only every k-th frae (to eulate faster caera otions). TABLE II: Evaluation of alternative weighting functions. Fig. 7: 3D reconstruction using an autonoous quadrocopter. Top: AscTec Pelican platfor used. Botto: Resulting 3D reconstruction of the roo coputed in real-tie on the ground station. better results on ost sequences. In coparison to RGB-D SLAM, we achieve often a siilar perforance in ters of accuracy but require six to eight ties less coputation tie. As our approach only uses structure for tracking, it fails in cases where only co-planar surfaces are visible, such as a wall. KinFu suffers the sae liitation, while RGB-D SLAM uses the texture. It would be interesting to additionally exploit the color inforation during tracking [12]. C. Paraeter Study We investigated the influence of the truncation paraeter δ and on the Teddy and Desk sequence. As Figure 8a shows, choosing δ too sall results in poor tracking. This is expected since then the band where the gradient is non-zero is very narrow. The upper liit clearly depends on the scene and encodes a prior of the average object depth. For typical office scenes, δ = 0.3 is a reasonable choice. Furtherore, we evaluated the weighting functions proposed in Section IV-C, Table II gives the results. We found that the exponential weighting function leads to ore robust tracking, although the linear weight is slightly ore accurate. Clearly, Dataset F1 Teddy RMSE Max Exp. Weight Linear Weight Constant Weight Narrow Exp. Narrow Linear Narrow Constant F1 Desk RMSE Max the narrow weighting functions yield inferior tracking results. D. Robustness Evaluation We also evaluated the robustness of our ethod with respect to the caera speed. For this, we tracked only every k-th frae on the Desk and Household sequences. The results are given in Figure 8b. The perforance quickly decreases if ore than three iages are left out in the fast Desk sequence (0.4 /s), while our approach yields decent results for up to six skipped fraes for the slower Household sequence (0.25 /s). The RMSE for every sixth iage is 4.3 c which still outperfors KinFu with 6.1 c when using every iage. E. Runtie and Meory Consuption For a resolution of = 256, our approach consues on average around 23 s and thus runs easily in real-tie tie on 30 fps RGB-D data. In coparison, KinFu consues on the sae hardware and at the sae resolution 20 s per frae, while RGB-D SLAM requires around s. All three algoriths ake extensive use of the GPU. Based on this

8 evaluation, we conclude that our algorith runs approxiately at the sae speed as KinFu and six ties faster than RGB-D SLAM. In ore detail, our approach requires 19.4 s for pose optiization and 3.7 s for data fusion at = 256, while for = 512, it takes 31.1 s for pose optiization and 21.6 s for data fusion. Note that in theory, the coplexity of pose optiization solely depends on the size of the input iages, while the coplexity of data fusion depends cubically on the resolution of the volue. For = 256, our approach requires 128 MB of RAM on the GPU for the SDF and 256 MB for the color grid; for = 512, it requires 1 GB for the SDF and 2 GB for the color grid. VI. CONCLUSION In this paper we presented a novel approach to directly estiate the caera oveent using a signed distance function. Our ethod allows the quick acquisition of textured 3D odels that can be used for real-tie robot navigation. By evaluating our ethod on a public RGB-D benchark, we found that it outperfors ICP-based ethods such as KinFu and, at least on ediu-sized scenes, often obtains a coparable perforance with bundle adjustent ethods such as RGB-D SLAM at a significantly reduced coputational effort. In the future, we plan to include color inforation in caera tracking and investigate ethods that allow a ore efficient representation of the 3D geoetry. For larger geoetries, the cobination of our ethod with a SLAM solver like [15, 11] would be interesting. REFERENCES [1] KinectFusion Ipleentation in the Point Cloud Library (PCL). [2] S. Agarwal, N. Snavely, I. Sion, S.M. Seitz, and R.Szeliski. Building roe in a day. In ICCV, [3] J. A. Bærentzen. On the ipleentation of fast arching ethods for 3D lattices. Technical report, Technical University of Denark, [4] P.J. Besl and N.D. McKay. A ethod for registration of 3-D shapes. IEEE Trans. Pattern Anal. Mach. Intell., 14 (2): , [5] G. Blais and M.D. Levine. Registering ultiview range data to create 3D coputer objects. IEEE Trans. Pattern Anal. Mach. Intell., 17: , [6] D. Canelhas. Scene representation, registration and object detection in a truncated signed distance function representation of 3d space. Master s thesis, [7] B. Curless and M. Levoy. A voluetric ethod for building coplex odels fro range iages. In SIGGRAPH, [8] F. Endres, J. Hess, N. Engelhard, J. Stur, D. Creers, and W. Burgard. An evaluation of the RGB-D SLAM syste. In ICRA, May [9] P. Henry, M. Krainin, E. Herbst, X. Ren, and D. Fox. RGB-D apping: Using depth caeras for dense 3D odeling of indoor environents [10] H. Hirschüller. Accurate and efficient stereo processing by sei-global atching and utual inforation. In CVPR, [11] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and Frank Dellaert. isam2: Increental soothing and apping with fluid relinearization and increental variable reordering. In ICRA, [12] C. Kerl, J. Stur, and D. Creers. Robust odoetry estiation for rgb-d caeras. In ICRA, [13] G. Klein and D. Murray. Parallel tracking and apping for sall AR workspaces. In ISMAR, [14] D.B Kubacki, H.Q. Bui, S.D. Babacan, and M.N. Do. Registration and integration of ultiple depth iages using signed distance function. In SPIE, Coputational Iaging X, [15] R. Küerle, G. Grisetti, H. Strasdat, K. Konolige, and W. Burgard. g2o: A general fraework for graph optiization. In ICRA, [16] Willia E. Lorensen and Harvey E. Cline. Marching cubes: A high resolution 3D surface construction algorith. Coputer Graphics, 21(4): , [17] Y. Ma, S. Soatto, J. Kosecka, and S. Sastry. An Invitation to 3D Vision: Fro Iages to Geoetric Models. Springer Verlag, [18] R.A. Newcobe, S. Izadi, O. Hilliges, D. Molyneaux, D. Ki, A.J. Davison, P. Kohli, J. Shotton, S. Hodges, and A.W. Fitzgibbon. KinectFusion: Real-tie dense surface apping and tracking. In ISMAR, pages , [19] C.Y. Ren and I. Reid. A unified energy iniization fraework for odel fitting in depth. In Workshop on Consuer Depth Caeras for Coputer Vision (CDC4CV) at ECCV, [20] F. Steinbrücker, J. Stur, and D. Creers. Real-tie visual odoetry fro dense rgb-d iages. In Workshop on Live Dense Reconstruction with Moving Caeras at ICCV, [21] J. Stüher, S. Guhold, and D. Creers. Real-tie dense geoetry fro a handheld caera. In Pattern Recognition (Proc. DAGM), Darstadt, Gerany, [22] J. Stur, N. Engelhard, F. Endres, W. Burgard, and D. Creers. A benchark for the evaluation of RGB-D SLAM systes. In IROS, [23] T.M. Tykkälä, C. Audras, and A.I Coport. Direct iterative closest point for real-tie visual odoetry. In Workshop on Coputer Vision in Vehicle Technology at ICCV, [24] T. Whelan, H. Johannsson, M. Kaess, J.J. Leonard, and J.B. McDonald. Robust real-tie visual odoetry for dense RGB-D apping. In IEEE Intl. Conf. on Robotics and Autoation, ICRA, Karlsruhe, Gerany, May [25] K.M. Wur, A. Hornung, M. Bennewitz, C. Stachniss, and W. Burgard. OctoMap: A probabilistic, flexible, and copact 3D ap representation for robotic systes. In ICRA, 2010.