GravityFusion: Real-time Dense Mapping without Pose Graph using Deformation and Orientation

Transcription

1 GravityFusion: Real-tie Dense Mapping without Pose Graph using Deforation and Orientation Puneet Puri 1, Daoyuan Jia 2, and Michael Kaess 1 Abstract In this paper, we propose a novel approach to integrating inertial sensor data into a pose-graph free dense apping algorith that we call GravityFusion. A range of dense apping algoriths have recently been proposed, though few integrate inertial sensing. We build on ElasticFusion, a particularly elegant approach that fuses color and depth inforation directly into sall surface patches called surfels. Traditional inertial integration happens at the level of caera otion, however, a pose graph is not available here. Instead, we present a novel approach that incorporates the gravity easureents directly into the ap: Each surfel is annotated by a gravity easureent, and that easureent is updated with each new observation of the surfel. We use esh deforation, the sae echanis used for loop closure in ElasticFusion, to enforce a consistent gravity direction aong all the surfels. This eliinates drift in two degrees of freedo, avoiding the typical curving of aps that are particularly pronounced in long hallways, as we qualitatively show in the experiental evaluation. I. INTRODUCTION Cobining dense 3D apping with inertial sensing provides significant advantages over pure vision-based apping. Dense 3D aps are relevant for obile robot navigation and anipulation, for augented and virtual reality applications, and for inspection tasks. And while ost dense apping systes rely on only stereo or RGB-D caeras, inertial sensors are becoing ubiquitous because of their low cost and wide range of applications. In the absence of a global reference such as GPS, apping algoriths suffer fro drift, which can partially be itigated by loop closures in a siultaneous localization and apping (SLAM) context. Drift results fro the accuulation of noisy sensor data over tie, where sall errors add up over longer trajectories to yield significant discrepancies between the odel and the physical world. A typical solution to reducing drift is loop closure: When we re-observe a previously visited area of the environent, we can create a constraint between the earlier observations and the current sensor data, a loop closure, that allows reoval of soe of the accuulated drift. Incorporating loop closures into dense apping requires the right odel representation: Voluetric representations are not suitable, as translation and rotation of parts of the odel is coputationally very expensive. Surface esh representation on the other hand can be defored at relatively low cost, but re-integrating two surface eshes after loop 1 P. Puri and M. Kaess are with the Robotics Institute, Carnegie Mellon University, Pittsburgh, PA 15213, USA puneetp@andrew.cu.edu, kaess@cu.edu 2 D. Jia is with the Language Technology Institute, Carnegie Mellon University, Pittsburgh, PA 15213, USA daoyuanj@cs.cu.edu Fig. 1. GravityFusion produces a consistent odel through a low-feature corridor circuit, where other ethods fail. closure is challenging. The surfel representation used in ElasticFusion allows for both, efficient deforation and reintegration upon loop closure. We therefore use ElasticFusion as the basis for our work. Even with loop closures, soe drift reains, and additional easureents, such as fro an inertial sensor, are needed to obtain accurate odels. When apping a single floor of a building there is typically insufficient data to constrain drift in soe directions, yielding a curved ap. The poorly constrained degrees of freedo include pitch, roll and z, while drift in x, y and yaw is bounded by the loop closures. Incorporating additional sensor inforation can help in liiting that drift. Inertial sensors provide copleentary inforation: by providing an estiate for the gravity direction, they eliinate drift in pitch and roll. Our contribution is a novel algorith that integrates inertial easureents directly into the ap. Since the ElasticFusion algorith is pose-free, conventional inertial fusion algoriths cannot be applied. Instead, we fuse inertial easureents inside the ap. We then use a esh deforation technique, used in ElasticFusion for loop closing, to enforce a consistent gravity direction over the entire ap, and thereby eliinating uch of the drift fro the ap. II. RELATED WORK Various dense apping techniques have been proposed over the past several years. One approach, BundleFusion by Dai et al. [2], uses bundle adjustent for global frae alignent cobined with dense ap generation, aking it very accurate but coputationally expensive. Probabilistic forulations account for estiation uncertainty and include REMODE by Pizzoli et al. [14] as well as planar apping

2 Fig. 2. Dataset of oving the caera through a long corridor. (Top) Kintinuous/RGBD SLAM [18] generates a ap that bends. (Middle) ElasticFusion [17] accuulates drift and loses tracking. (Botto) Our approach, GravityFusion, frequently defors the odel using the easured gravity direction to produce a consistent odel. approaches such as [5] that use infinite planes in a graph optiization to odel planar surfaces. Another set of approaches are based on an iplicit surface representation in the for of a signed distance function, a voluetric representation that allows fusion of ultiple caera fraes. The first real-tie syste is KinectFusion by Newcobe et al. [13], which was later extended in Kintinuous to large scale environents by Whelan et al. [18]. Another coon approach called pointbased fusion by Keller et al. [7] fuses sensor inforation into sall surface patches with orientation and scale that are called surfels. ElasticFusion by Whelan et al. [17] cobines point-based fusion with esh deforation for large scale loop closure. Because it allows for reintegration of the odel after loop closure, we use ElasticFusion as the basis for our inertial apping syste. Inertial sensor easureents are often integrated into apping in the context of feature-based ethods, soe exaples follow. Indelan et al. [6] provide a soothing-based solution to IMU integration with other sensors in a factor graph fraework. Forster et al. [3] iprove upon the preintegration ethod used for integrating the inertial data into the SLAM factor graph. Leutenegger et al. [9] also provide an integrated SLAM solution based on soothing. Mur-Artal and Tardos [12] integrate the inertial easureents during tracking over a local ap. Inertial sensing integrated with dense apping is less coon to find in the literature. One way is to use a featurebased inertial integration, such as Mur-Artal and Tardos [12], to recover the caera trajectory, and then apply a separate dense apping syste, such as Mur-Artal and Tardos [11]. Concha et al. [1] present a direct visual SLAM ethod fused with inertial easureents. Usenko et al. [16] recently presented a dense visual inertial odoetry ethod, however, by only using odoetry it is not possible to incorporate loop closures for consistent large scale apping. Ma et al. [10] present a KinectFusion-based inertial fusion, however, without the capability to close loops as a deforation of the dense voluetric representation is coputationally expensive. To retain the advantages of fusion while allowing loop closures, we build on point-based fusion, and in particular the ElasticFusion algorith. While all of the above ethods incorporate inertial sensor data at the level of caera otion, we present a novel approach that directly fuses the with the ap. In our GravityFusion algorith, correction of the ap proceeds in uch the sae way as loop closing in ElasticFusion, using a deforation graph [15] to perfor esh deforation. III. APPROACH GravityFusion corrects accuulated drift in the ap using inertial inforation ebedded in surfels. As the caera oves, the syste builds a ap of surfels siilar to ElasticFusion. However, instead of just fusing RGB-D data fro ultiple fraes to create a surfel, GravityFusion also includes orientation inforation fro an inertial sensor at the tie of frae capture. This is done by including the easured direction of earth s gravity into the surfel that is being created or updated. Siilar to updating position, orientation, and color of a surfel in ElasticFusion, a weighted average is used to fuse gravity vectors of a surfel easured at different ties. The ap created as a result has easured gravity vectors ebedded in every surfel. While the apping is being done, the GravityFusion syste triggers gravity vector realignent after a certain nuber of fraes to ensure that the ap s drift is corrected. This correction is done using a esh deforation graph, siilar to ElasticFusion. The esh nodes are sapled surfels of the ap. These nodes, like surfels, contain gravity inforation. The nodes of the deforation graph are reoriented to align the gravity vectors to a coon direction, while also taking odel constraints into account. Subsequently, all surfels that are not part of the deforation graph are corrected based on neighboring deforation graph nodes. The drift correction of a node is propagated to its neighbouring surfels which reorients and repositions the to ensure correct alignent. The extent of correction

3 is based on vicinity to a correcting graph node. It is possible that ultiple nodes affect a surfel and contribute to its reorientation and repositioning. Since these corrections are done for all surfels throughout the ap, in the end we get a ap which is drift corrected. Frequent gravity-based ap corrections are perfored in real-tie. While ElasticFusion applies graph deforation only at loop closure, we regularly perfor a esh deforation to incorporate gravity easureents and eliinate drift. To allow for real-tie processing, the gravity correction is added to the GPU-based loop closure algorith in Elastic- Fusion. Consequently, a good ap is always available in our pose-graph-free approach, without the need to aintain past states or deal with issues often related to the arginalization of previous state inforation. IV. MAP CREATION The apping process in GravityFusion resebles Elastic- Fusion s with changes in caera tracking ethod and ap data structure. The ap is represented as an unordered surfel list siilar to Whelan et al. [17] and Keller et al. [7]. A surfel S consists of gravity vector g R 3, position v R 3, noral n R 3, color c N 3, confidence λ R, radius r R, initialization tiestap t 0 and last updated tiestap t. Sections A and B explain how inertial inforation is integrated into the tracking and apping systes. A. Tracking Our caera tracking approach closely resebles Elastic- Fusion s [17] where cobined depth tracking and photoetric alignent is initialized with photoetric alignent only. Our approach differs as it infors this photoetric alignent with an inertial easureent fro the IMU rotation, thus leading to a ore robust caera pose estiation. We only ake use of rotation values fro the inertial data because increental angular otion is available with greater accuracy using standard IMUs. In the RGB-D incoing frae we define the iage doain as Ω N 2 and the depth ap D as the depth of pixels d : Ω R. The 3D back-projection of a point u Ω for a given depth ap D is given by p(u,d) = K 1 ud(u), where K is the caera intrinsic atrix and u is the hoogeneous for of u. The perspective projection of a 3D point p = [x,y,z] (in caera frae F c ), is represented as u = π(kp). Here π(p) = [x/z,y/z] represents the de-hoogenization operation. The noral ap is coputed for every depth ap using central differences. The color iage C is represented as c: Ω N 3. The color-intensity value of a pixel u Ω, given a color iage C with color c(u) = [c 1,c 2,c 3 ], is defined as I(u,C) = (c 1 +c 2 +c 3 )/3. The global pose of the caera P t (in global frae F g ) is deterined by live registration of a depth ap and a color frae with the odel. The pose estiation iteratively iniizes tracking error E tracking as a weighted su of geoetric error E icp and photoetric error E rgb. These errors are calculated between the raw live depth ap D t fro the sensor and the active odel s depth ap using the last frae, ˆD t 1, in order to find the optial otion paraeter ξ, where ξ R 6 and exp(ξ ) SE(3). The tracking error is expressed as E tracking = E icp + ω rgb E rgb, (1) E icp = ((v k exp(ξ )T.vt k ).n k ) 2, (2) k E rgb = (I(u,C t I(π(K exp(ξ )Tp(u,D t )),C t 1 ))) 2, (3) u Ω where ω rgb is the weight of photoetric error E rgb over ICP error E icp. Here, vt k is the back-projection of the k th vertex in D t, v k represents the corresponding vertex in the odel, and n k is the noral of v k. T is the current estiation of transforation of caera pose and exp(ξ ) is the increental otion with paraeter ξ to be optiized in the current iteration. Siilarly the color fro the live frae C t at the current tie t and odel C t 1 is used to find the optiu otion paraeter ξ using photoetric error the intensity difference between pixels. For ore detailed understanding of the above equations we refer to Whelan et al. [17]. Siilar to ElasticFusion, the energy ters fro (2) and (3) are jointly optiized to calculate the least squares solution for the optial ξ to get an accurate caera pose estiate P t = exp(ξ )TP t 1. Unlike ElasticFusion, before the optiization begins, the initial estiate T is odified to incorporate the easured inertial-orientation R iu SO(3) for a ore accurate initialization of T. We call this the SO(3) initialization step. B. Adding Inertial Inforation into Surfels The inertial inforation is incorporated into the ap by ebedding gravity direction into every surfel. This is done as the frae is being aligned and fused to the odel. To orient the gravity vector into the odel frae, a unit vector [0,0,1], in odel frae, is transfored into the IMU (caera) frae using the inertial readings fro the IMU. The inertial data (orientation) is obtained as filtered output of an attitude heading reference syste (AHRS) of the IMU, denoted as R iu. We denote this gravity vector in IMU frae as g i iu R3 where i R is the surfel s index. We then transfor g i iu back to the odel frae using the tracking estiate of the caera orientation represented as R tracking SO(3). We denote this gravity vector as g i odel. The g i odel is added into every surfel, providing constraints on two degrees of freedo fro the inertial easureents. g i iu = (R iu ) 1 [ ] (4) g i odel = R trackingg i iu (5) C. Updating Gravity Direction Inforation When new fraes are fused to the odel, surfels are updated for gravity, position, noral and radius according to the confidence of the current and incoing surfels, siilar to ElasticFusion. The surfel correspondences are built with ap registration of the incoing frae. A confidence value λ is initialized along with the creation of the surfel and accuulates each tie we observe the sae surfel in the

4 caera frae. The confidence paraeter acts as a weight: a easure of the extent to which we should retain the surfel in an update. The update equations for the surfel paraeter, given the new incoing surfel s paraeters, gravity direction g, position v, noral n and confidence λ are: ĝ = λ g + λ g λ + λ (6) ˆn = λ n + λ n λ + λ (7) ˆv = λ v + λ v λ + λ (8) ˆλ = λ + λ (9) The updated gravity direction g of the surfel is a weighted average of the current estiate with the gravity reading of the incoing surfel g. V. MAP CORRECTION: GRAVITY BASED DEFORMATION GRAPH To correct the odel for drift and tracking inconsistencies we use a deforation graph. The deforation odel used here is general enough to be applied to any ap and still provides intuitive anipulation while preserving surface consistency. This forulation allows stretching and realignent of the ap while aintaining surface continuity. The nodes of the deforation graph are created by sapling existing surfels fro a full odel. As a result, the graph created is a sparse odel of the original ap. The deforation of this graph is a set of affine transforations of graph nodes providing spatial reorganization. These affine transforation influence the neighboring nodes, having an overlapping effect. Our deforation graph is siilar to that in Suner et al. [15] and Whelan et al. [17] with an essential addition that each node contains inertial inforation in the for of gravity direction. A single graph node contains a gravity vector G g R 3, its position G p R 3 and a tiestap. The graph node also stores an affine transforation as rotation G R SO(3) and translation G t R 3 to be applied to itself, while also influencing surfels and graph nodes in its neighborhood. This affine transforation is initialized as G t = 0 and G R = I at the tie of graph creation, and again after copletion of graph optiization. We consider the total nuber of graph nodes to be and each having at ost K neighbors. As explained in below subsections, the influence of graph nodes can be broken down into two stages: firstly, how graph nodes affect surfels in their vicinity, as shown in Fig. 3 and secondly, how graph nodes influence each other during graph optiization for deforation as shown in Fig. 4. A. Influence of Deforation Graph on Surfels An affine transforation of a graph node is centered around itself. As a graph node rotates (G R ) and translates (G t ), its influence causes the nearby surfels to spatially reorient. The effect of graph node s affine transforation on a nearby surfel at location v is shown in Fig. 3. This principle Fig. 3. Influence of affine transforation (G R,G t ) of a graph node (in green) on a neighboring surfel (in blue) by spatially reorienting the surfel to a new location. is used throughout the deforation graph to obtain the new location ˆv given by: ˆv = G R (v G p ) + G p + G t (10) The extent of this influence is liited by the distance of surfel i fro the graph node j. The influence here is represented as a weight w j w j(vi) = (1 vi G j p /d ax ), (11) where d ax is the distance of the surfel to the K th nearest graph node. To soothly blend the effect of ultiple graph nodes on a surfel, we su over the cobined influence. The resultant final position of the surfel is written as ˆv i = w j (v i )G j R (v i G j p) + G j p + G j t (12) Any rotation applied to a surfel effect the orientation of its noral n i. Siilar to the position update, the cobined influence of graph node s rotation on the surfel s orientation is expressed as ˆn i = w j (v i )(G j R ) 1 n i (13) As surfel and its noral orientation get updated, a siilar update is applied to the gravity direction associated with that surfel. This update ensures that after deforation the gravity direction reains consistent to the surfel s orientation. ĝ i = w j (v i )(G j R ) 1 g i (14) We saple the graph nodes densely, ensuring that graph nodes have an accurate representation of the entire ap. This dense sapling assures that when deforation is applied, its influence reaches all desired surfels in the ap. B. Optiization of Deforation Graph We optiize the deforation graph to find the affine transforation of graph nodes, which when applied to our odel will correct its drift and ake it consistent. This optiization utilizes the direction of gravity vectors in the

5 Fig. 4. We highlight here the effect of each energy ter on the spatial orientation of the graph nodes in deforation graph. The top iage (i) shows constraint energy ter of the type pin constraint E pin. The E pin freezes the graph node in its location (in red), acting as a standard reference. The iddle figure (ii) shows the effect of the realignent of the gravity vectors caused by the additional E gravity ter. The botto figure (iii) shows the effect of inclusion of regularization E reg ter. The E reg causing the graph nodes to reain consistent with each other, enforcing a surface geoetry, and keeping the odel consistent. graph nodes to infor the deforation. In an ideal case where there is no drift, all of the gravity vectors on each of the graph nodes will be parallel to each other. However, since the odel accuulates drift over tie, the nodes gravity vectors becoe isaligned as shown in Fig. 4 (i), the optiization explained below penalizes this inconsistent orientation. The graph optiization finds the unknown variables which describe a corrective rotation and translation of every graph node. These rotations and translations, when applied back to graph nodes and in turn onto surfels, addresses the drift by correcting the odel. The optiization of the deforation graph is done using iniization of the cobined cost of four coponents: 1) Constraints: The constraints cost ter E con, ensures that these graph nodes are either allowed to ove or to reain stationary. A single constraint on a graph node l is a tuple containing its transfored source location as φ(g l p(source) ) and its destination location as Gl p(dest). These constraints are of three types: A) Pin constraint: They freeze graph nodes at their position, aking the unable to rotate or translate during graph deforation. This constraint type is expressed in (15) as E pin = E con. Here the graph node s source and destination location are kept identical. B) Generic constraint: The graph nodes having a generic constraint, spatially re-locate fro their source location to their destination location. Generic constraints are used for loop closures as they defor the ap fro its current location (source) to a siilar previously visited location (destination). The syste ensures that graph nodes at destination location reain fixed and do not snap towards source location by adding a pin constraint on destination graph node. C) Relative constraint: This type of constraint is siilar to a generic constraint with the exception that neither source graph node nor destination graph node are constrained via a pin constraint. The relative constraint is required in to ensure that previous loop closures reain consistent and are not torn off due to newly added generic constraints. Out of the three constraint types, our approach uses pin constraints actively to freeze certain graph nodes, locking their gravity vectors in place to serve as a reference. E con = φ(g l p(source) ) G l 2 p(dest) (15) 2 l 2) Gravity Alignent: In the process of ap creation, the odel accuulates drift due to sall errors in tracking and the gravity vectors diverge as shown in Fig. 4 (i). These vectors in a drifted odel are not entirely parallel. The gravity alignent cost ter E grav penalizes this incorrect alignent and iniizes the alignent error by realigning gravity vectors to a standard reference. The standard reference is the direction of gravity vector G k g, fro a graph node k with a pin constraint, shown in red in Fig. 4. This standard reference graph node reains frozen in its orientation and location and is usually sapled fro the recently acquired parts of the odel. The graph optiization results in a corrective rotation G j R required for every graph node j in the odel, such that gravity vectors of these nodes becoe parallel as shown in (ii) of Fig. 4. E grav = j k (G j R G g) j.g k g 1 2 (16) 2 3) Regularization: The regularization ter is added to the graph optiization to ensure that the odel reains consistent and the surface geoetries are enforced. To highlight this, Fig. 4 (ii) shows the effect of inconsistent geoetry while not using regularization, and (iii) shows a ore consistent geoetry with the inclusion of regularization ter. The distortion free ap deforation is achieved by ensuring overlapping influence of affine transforation fro neighbouring graph nodes to be consistent to one another. The regularization ter E reg is described in (17), where graph node j influences affine transforation of graph node k, with k one of the neighboring nodes N ( j). The cost E reg is calculated as a su of squared difference between the position of graph node k predicted by influence fro graph node j and the position of graph node k when its affine transforation is applied to itself. The weight α jk is proportional to the degree of which the influence of nodes j and k overlap. For ore details, we refer the reader to Suner et al. [15]. E reg = k N ( j) α jk G j R (Gk p G j p)+g j p +G j t (G k p +G k t ) 2 2 (17) 4) Rotation: Lastly we want to ensure that the optiized variables are orthonoral to for a valid rotation atrix,

6 Fig. 5. Our caera setup: ASUS Xtion Pro Live with Microstrain 3DM- GX4-25. which is enforced by E rot = (G j R ) (G j R ) I 2 (18) 2 The coplete graph deforation is based on optiization of a cobined su of all the coponents explained above. The weight coefficients control the effect of each energy ter: in G 1 R..G R, G 1 t..g t ω con E con + ω reg E reg + ω rot E rot + ω grav E grav (19) In our experients, we set ω rot = 1, ω reg = 10, ω con = 100, and ω grav = 100. We iniize the weighted energy ter w.r.t each graph node s rotation G j j R, j and translation Gt, j using an iterative Gauss-Newton ethod. The rotation and translation calculated are used in (12), (13), and (14) to align the odel for correction. C. Triggering Graph Optiization We trigger graph optiization to correct for drift after a fixed interval of fraes (C t =500). This frequent graph optiization ensures that gravity constraints are enforced even if no loop closure occurs. In the event of local and global loop closure, we trigger graph optiization siilar to ElasticFusion. For details on local and global loop closure we refer to Whelan et al. [17] VI. RESULTS We evaluate the perforance of our syste qualitatively using data fro different real-world environents such as hallways and office alleys. We also present a quantitative surface evaluation on a public dataset in subsection B. A. Evaluation on Real-world Data Our hardware setup is a cobination of an RGB-D caera (ASUS Xtion Pro Live) and an IMU (Microstrain 3DM- GX4-25) as shown in Fig. 5. We use filtered output of inertial readings fro a Microstrain 3DM-GX4-25 sensor. We focus qualitative testing on long corridors instead of sall roo environents. In long hallway type environent often dense ethods without pose graph fail, as slow drift in ap registration causes bending, or lack of features cause loss of tracking due to incorrect photoetric alignent. We showcase our syste s versatility as it perfors well in various environents such as: a) Long corridor with features b) Corridors with fewer features c) Corridors with loops and d) Office desk environent. 1) Experient for Drift Correction: We capture data through a long corridor which has significant features. Fig. 2 shows the coparison aong odels generated fro various systes. The Kintinuous/RGB-D SLAM [18] in Fig. 2 (top) tracks successfully, however, due to drift accuulation, there is a significant bend in the corridor ap. The ElasticFusion in Fig. 2 (center) suffers both fro drift accuulation and loss of tracking. Our syste in Fig. 2 (botto) creates a consistent ap, generating a straight corridor. 2) Experient for Tracking Correction: For qualitative evaluation of tracking, we do a 4-way coparison where the IMU based SO(3) initialization of caera tracking is added to ElasticFusion, as described in the tracking section IV.A. In Fig. 6, the first iage (fro top) shows tracking failure of ElasticFusion due to the incorrect photoetric alignent. The second iage shows the ap created fro ElasticFusion (with IMU based tracking initialization). Despite the initialization, the syste converges incorrectly and loses tracking. The third iage shows the ap fro GravityFusion (without IMU based tracking initialization). The ap deforation is perfored using gravity inforation and the odel recovers despite tracking loss, though soe isalignent of the corridor reains and is visible as an artifact in the center. The fourth iage is of our syste GravityFusion (with IMU based tracking initialization). Here our syste itigates tracking loss and creates a consistent odel with no visible artifacts. 3) Experient for Loop Closures: To test loop closure perforance, we go through an office environent with alleys connected in a circuit. The top iage in Fig. 7 shows a ap generated using ElasticFusion. ElasticFusion is unable to track correctly and bends the odel creating an inconsistent ap. The center iage shows a ap generated fro the Kintinuous/RGB-D SLAM syste. Near alley corners, the Kintinuous/RGB-D SLAM syste is unable to estiate caera rotation correctly, resulting in an inconsistent odel. The botto iage is of our syste, as it tracks accurately following through corridors and corners without bending. Though we do not trigger a global loop closure, our syste triggers ultiple local loop closures, creating a ore consistent odel. 4) Experient for Office Environents: In this experient we test our syste ensuring that we retain the ElasticFusion-like ability to do apping with loopy (and zigzag) caera otion. Here we show a cluttered office desk roo which is apped with siilar caera otion. We first do loopy (and zigzag) caera oveent to capture the desk and then ove the caera around through open doors to arrive back at starting location. Our syste handles this caera otion well and generates a consistent ap, shown in Fig. 8. B. Evaluation on Siulated Dataset Since our syste perfors frequent graph optiization, which defors and fixes the odel, we here verify that the resulting ap does not have any undesired bends or artifacts. We evaluate our syste on the ICL-NUIM dataset by Handa

7 Fig. 6. Coparison of caera tracking loss and recovery on a corridor dataset for the following systes, fro the top: 1) ElasticFusion, loses tracking. 2) ElasticFusion with tracking initialized with inertial estiate, still loses tracking. 3) Our approach of GravityFusion without inertial initialization of the caera tracking. It recovers the broken odel using deforation, however leaves artifacts in iddle of the corridor. 4) Our approach of GravityFusion with inertial initialization of the caera tracking. GravityFusion recovers the odel using deforation while aintaining ap consistency. Syste lr kt0 lr kt1 lr kt2 lr kt3 DVO SLAM [8] Kintinuous ElasticFusion GravityFusion TABLE I ICL-NUIM DATASET PERFORMANCE OF VARIOUS SYSTEMS et al. [4]. We copute the accuracy of the generated ap against the 3D surface fro the dataset. Since no IMU value is provided in these datasets, we use the caera trajectory s ground-truth to siulate inertial inforation by adding Gaussian noise (σ =0.5 degrees). We evaluate on all four datasets, and the results of these surface evaluation errors are listed in Table I. Our syste not only atches the odel accuracy of the copared systes, but we also exceed their accuracy on two of the datasets: lr kt1 and lr kt3. VII. CONCLUSIONS We have presented a novel algorith for correcting a ap with inertial sensor data in the absence of a pose graph. Our GravityFusion algorith fuses inertial easureents of gravity on a per-surfel basis, and enforces global consistency of the gravity direction through a esh deforation. Our algorith eliinates drift in pitch and roll. Questions for future research include how to ake full use of the inertial easureents, i.e. what is the equivalent of tightly coupled integration into the ap. At the tracking level inertial inforation could be used not just for initialization, but also as a constraint in the alignent optiization, which could provide helpful constraints in perceptually abiguous settings. ACKNOWLEDGMENT This work was partially supported by the National Science Foundation, award nuber We would like to thank Vishal Dugar and Rohan Thakker for ipleenting parts of the SO(3) initialization. We would also like to thank Thoas Whelan for patiently answering any of our questions on ElasticFusion. REFERENCES [1] A. Concha, G. Loianno, V. Kuar, and J. Civera. Visual- Inertial Direct SLAM. In: IEEE Intl. Conf. on Robotics and Autoation (ICRA) [2] A. Dai, M. Nießner, M. Zollöfer, S. Izadi, and C. Theobalt. BundleFusion: Real-tie Globally Consistent 3D Reconstruction using On-the-fly Surface Re-integration. In: ACM Transactions on Graphics 2017 (TOG) (2017). [3] C. Forster, L. Carlone, F. Dellaert, and D. Scarauzza. On-Manifold Preintegration for Real-Tie Visual-Inertial Odoetry. In: IEEE Trans. Robotics (2016).

8 Fig. 8. Map of a cluttered office deskroo using loopy (and zigzag) caera otion. Fig. 7. Map generated fro a dataset of office alleys in a circuit by following systes: (Top) ElasticFusion loses tracking. (Middle) Kintinuous incorrectly estiates caera rotation at alley corners, losing tracking. (Botto) GravityFusion aintains a straight floor profile creating a consistent odel. [4] A. Handa, T. Whelan, J. McDonald, and A. Davison. A Benchark for RGB-D Visual Odoetry, 3D Reconstruction and SLAM. In: IEEE Intl. Conf. on Robotics and Autoation, ICRA. Hong Kong, China, May [5] M. Hsiao, E. Westan, G. Zhang, and M. Kaess. Keyfraebased Dense Planar SLAM. In: IEEE Intl. Conf. on Robotics and Autoation (ICRA). To appear. Singapore, May [6] V. Indelan, S. Wilias, M. Kaess, and F. Dellaert. Inforation Fusion in Navigation Systes via Factor Graph Based Increental Soothing. In: Robotics and Autonoous Systes 61.8 (2013), pp [7] M. Keller, D. Lefloch, M. Labers, S. Izadi, T. Weyrich, and A. Kolb. Real-tie 3D Reconstruction in Dynaic Scenes using Point-based Fusion. In: Proc. of Joint 3DIM/3DPVT Conference (3DV) [8] C. Kerl, J. Stur, and D. Creers. Dense Visual SLAM for RGB-D Caeras. In: Proc. of the Int. Conf. on Intelligent Robot Systes (IROS) [9] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale. Keyfrae-based Visual-inertial Odoetry using Nonlinear Optiization. In: Intl. J. of Robotics Research 34.3 (2015), pp [10] L. Ma, J. Falquez, S. McGuire, and G. Sibley. Large Scale Dense Visual Inertial SLAM. In: Field and Service Robotics (FSR). 2016, pp [11] R. Mur-Artal and J. Tardos. Probabilistic Sei-Dense Mapping fro Highly Accurate Feature-Based Monocular SLAM. In: Robotics: Science and Systes (RSS). July [12] R. Mur-Artal and J. Tardos. Visual-Inertial Monocular SLAM with Map Reuse. arxiv: [13] R. Newcobe, A. Davison, S. Izadi, P. Kohli, O. Hilliges, J. Shotton, D. Molyneaux, S. Hodges, D. Ki, and A. Fitzgibbon. KinectFusion: Real-Tie Dense Surface Mapping and Tracking. In: IEEE and ACM Intl. Sy. on Mixed and Augented Reality (ISMAR). Basel, Switzerland, 2011, pp [14] M. Pizzoli, C. Forster, and D. Scarauzza. REMODE: Probabilistic, Monocular Dense Reconstruction in Real Tie. In: IEEE Intl. Conf. on Robotics and Autoation (ICRA) [15] R. Suner, J. Schid, and M. Pauly. Ebedded Deforation for Shape Manipulation. In: SIGGRAPH. San Diego, California, [16] V. Usenko, J. Engel, J. Stückler, and D. Creers. Direct Visual-Inertial Odoetry with Stereo Caera. In: IEEE/RSJ Intl. Conf. on Intelligent Robots and Systes (IROS) [17] T. Whelan, S. Leutenegger, R. F. Salas-Moreno, B. Glocker, and A. J. Davison. ElasticFusion: Dense SLAM Without A Pose Graph. In: Robotics: Science and Systes (RSS). Roe, Italy, July [18] T. Whelan, M. Kaess, H. Johannsson, M. Fallon, J. Leonard, and J. McDonald. Real-tie Large Scale Dense RGB-D SLAM with Voluetric Fusion. In: Intl. J. of Robotics Research (Apr. 2015), pp