Variational State-Space Models for Localisation and Dense 3D Mapping in 6 DoF

1 INTRODUCTION . We address the problem of learning representations of spatial environments , perceived through RGB-D and inertial sensors , such as in mobile robots , vehicles or drones . Deep sequential generative models are appealing , as a wide range of inference techniques such as state estimation , system identification , uncertainty quantification and prediction is offered under the same framework ( Curi et al. , 2020 ; Karl et al. , 2017a ; Chung et al. , 2015 ) . They can serve as so-called world models or environment simulators ( Chiappa et al. , 2017 ; Ha & Schmidhuber , 2018 ) , which have shown impressive performance on a variety of simulated control tasks due to their predictive capability . Nonetheless , learning such models from realistic spatial data and dynamics has not been demonstrated . Existing spatial generative representations are limited to simulated 2D and 2.5D environments ( Fraccaro et al. , 2018 ) . On the other hand , the state estimation problem in spatial environments—SLAM—has been solved in a variety of real-world settings , including cases with real-time constraints and on embedded hardware ( Cadena et al. , 2016 ; Engel et al. , 2018 ; Qin et al. , 2018 ; Mur-Artal & Tardós , 2017 ) . While modern visual SLAM systems provide high inference accuracy , they lack a predictive distribution , which is a prerequisite for downstream perception–control loops . Our approach scales the above deep sequential generative models to real-world spatial environments . To that end , we integrate assumptions from multiple-view geometry and rigid-body dynamics commonly used in modern SLAM systems . With that , our model maintains the favourable properties of generative modelling and enables prediction . We use the recently published approach of Mirchev et al . ( 2019 ) as a starting point , in which a variational state-space model , called DVBF-LM , is extended with a spatial map and an attention mechanism . Our contributions are as follows : • We use multiple-view geometry to formulate and integrate a differentiable raycaster , an attention model and a volumetric map . • We show how to integrate rigid-body dynamics into the learning of the model . • We demonstrate the successful use of variational inference for solving direct dense SLAM for the first time , obtaining performance close to that of state-of-the-art localisation methods . • We demonstrate strong predictive performance using the learned model , by generating spatially-consistent real-world drone-flight data enriched with realistic visuals . • We demonstrate the model ’ s applicability to downstream control tasks by estimating the cost-to-go for a collision scenario . The contributions allow the reformulated model to tackle realistic RGB-D scenarios with 6 DoF . 2 RELATED WORK . Generative models for spatial environments GTM-SM ( Fraccaro et al. , 2018 ) focuses on longterm predictions with a non-metric deterministic external memory . Chaplot et al . ( 2018 ) formulate an end-to-end learning model for active global localisation , filtering with a likelihood update predicted by a neural network . The agent can turn in four directions and move on a plane , perceiving images of the environment . VAST ( Corneil et al. , 2018 ) assumes a discrete state space for a generative model applied to the 2.5D Vizdoom environment . Whittington et al . ( 2018 ) model agents moving on a 2D grid with latent neurologically-inspired grid and place cells . Other works propose end-to-end learnable generative scene models ( Eslami et al. , 2018 ; Engelcke et al. , 2020 ) , without considering the agent dynamics . Like in the above , we put major emphasis on the generative predictive distribution of our model . With it , the agent can imagine the consequences of its future actions , a prerequisite for data-efficient model-based control ( Chua et al. , 2018 ; Hafner et al. , 2019a ; b ; Becker-Ehmck et al. , 2020 ) . However , the aforementioned deep generative spatial models have only been applied on simulated 2D , 2.5D ( movement restricted to a plane ) and very simplified 3D environments . A major challenge when scaling to the real world is to ensure that the learned components , and in turn the generative predictions , generalise to observed but yet unvisited places . Gregor et al . ( 2019 ) highlight another problem , that of long-term consistency when predicting ahead , and address it by learning with overshooting . In contrast , our method resolves these issues by injecting a sufficient amount of domain knowledge , without limiting the flexibility w. r. t. learning . To this end , we begin by sharing the probabilistic factorisation of DVBF-LM ( Mirchev et al. , 2019 ) , a deep generative model that addresses the tasks of localisation , mapping , navigation and exploration in 2D . We then redefine the map , the attention , the states , the generation of observations and the overall inference , allowing for real-world 3D modelling and priming our method for data-efficient online inference in the future . We discuss why these changes are necessary more thoroughly in appendix A . Combining learning and spatial domain knowledge Fully-learned spatial models with an explicit memory component have been studied by Parisotto & Salakhutdinov ( 2018 ) ; Zhang et al . ( 2017 ) ; Oh et al . ( 2016 ) . Further relying on geometric knowledge , Tang & Tan ( 2019 ) propose learning through the whole bundle adjustment optimisation , formulated on CNN feature maps of the observed images . Czarnowski et al . ( 2020 ) define a SLAM system based on learned latent feature codes of depth images , a continuation of the works by Zhi et al . ( 2019 ) ; Bloesch et al . ( 2018 ) . Factor-graph maximum a posteriori optimisation is then conducted , substituting the observations for their respective low-dimensional codes , leading to point estimates of the individual geometry of N keyframes and the agent poses over time . Wei et al . ( 2020 ) maintain cost volumes ( Newcombe et al. , 2011 ) for discretised poses and depth , and let a 3D CNN learn how to predict the correct geometry and pose estimates from them . Depth cost volumes are also used by Zhou et al . ( 2018 ) in learning to predict depth and odometry with neural networks . In the work by Yang et al . ( 2020 ) , networks that predict odometry and depth are combined with DSO , leading to a SLAM system that utilises learning to its advantage . Jatavallabhula et al . ( 2019 ) investigate differentiable SLAM , treating odometry estimation and mapping separately . The considered rendering gradients in that method are from the fused map to the observations , which is the opposite of the gradient paths used for learning in our work . As in our approach , the geometric assumptions in the majority of these works allow the systems to generalise more easily to unseen cases and real-world data . What distinguishes our method is an explicit generative model , able to predict the agent movement and observations in the future . Additionally , our approach is fully-probabilistic , maintaining complete distributions over the variables of interest , whereas the aforementioned approaches are not . Our method is also end-to-end differentiable and can be implemented in auto-diff frameworks , welcoming learned components . We are bridging the gap between probabilistic generative models , learning and spatial domain knowledge . Depth estimation and differentiable rendering Recent promising approaches combine learning and non-parametric categorical distributions for depth estimation ( Laidlow et al. , 2020 ; Liu et al. , 2019 ) , fusing likelihood terms into a consistent depth estimate . Such depth estimation is compatible with our system and can be used to formulate priors , but for now we rely on a traditional method as a first step ( Hirschmuller , 2007 ) . Inferring whole scenes parameterised with neural networks by backpropagation through realistic differentiable rendering has also become a prevalent direction of research ( Bi et al. , 2020 ; Mildenhall et al. , 2020 ; Sitzmann et al. , 2020 ) . In our method the occupancy and colour map are inferred in a similar way , but the raycasting scheme we follow is simple , meant only to illustrate the framework as a whole . We note that the current inference times of e.g . Bi et al . ( 2020 ) amount to days ( see appendix of that work ) , which is hard to scale to online inference . Extending our approach with a more advanced rendering method is the subject of future work . Bayesian SLAM inference To keep exposition brief , we refer to ( Cadena et al. , 2016 ) for an overview of modern SLAM inference and focus only on approaches that have applied fully-Bayesian methods to SLAM . The inference in this work can be categorised as probabilistic SLAM , other prominent examples of which are FastSLAM ( Montemerlo et al. , 2002 ) and RBPF SLAM ( Grisetti et al. , 2005 ) . What distinguishes our method is the application of variational inference with SGVB ( Kingma & Welling , 2014 ) . Our model does not restrict the used distributions and allows any differentiable functional form , which enables us to use neural networks . The contribution by Murphy ( Murphy , 1999 ) is one of the first to infer a global map with Bayesian methods . Bayesian Hilbert maps ( Senanayake & Ramos , 2017 ) focus on a fully Bayesian treatment of Hilbert maps for long-term mapping in dynamic environments . Stochastic variational inference is used to infer agent poses from observed 2D image features in ( Jiang et al. , 2017 ; Jiang et al. , 2019 ) . DVBF-LM ( Mirchev et al. , 2019 ) uses Bayes by Backprop ( Blundell et al. , 2015 ) for the inference of the global map variable . 3 METHOD . Background We adhere to the graphical model of DVBF-LM ( Mirchev et al. , 2019 ) , but we introduce novel design choices for every model component and implement the overall inference differently , to allow for real-world 3D modelling . In the following , we will first describe the assumed factorisation and then explain the introduced modifications . The assumed joint distribution of all variables is : p ( x1 : T , z1 : T , m1 : T , M | u1 : T−1 ) = p ( M ) ρ ( z1 ) T∏ t=1 p ( xt |mt ) p ( mt | zt , M ) T−1∏ t=1 p ( zt+1 | zt , ut ) , ( 1 ) where x1 : T are observations , z1 : T agent states , m1 : T map charts and u1 : T−1 conditional inputs ( controls ) . The factorisation defines a traditional state-space model extended with a global map variable M. For a single step t , an observation xt is generated from a map chart mt—the relevant extract from the global M around the current agent pose zt ( cf . fig . 2a ) . Chart extraction is given by p ( mt | zt , M ) , which can be seen as an attention mechanism . In this graphical model , SLAM is equivalent to inference of the agent states z1 : T and the map M. For the remainder of this work , we assume all observations xt ∈ Rw×h×4 are RGB-D images . Next , we will describe the functional forms of the map M , the attention p ( mt | zt , M ) , the emission p ( xt |mt ) and the states z1 : T . Geometric map The map random variable M = ( Mocc , Mcol ) consists of two components . Mocc ∈ Rl×m×n is a spatially arranged 3D grid of scalar values that represent occupancy . Mcol represents the parameters of a feed-forward neural network fMcol : R3 → [ 0 , 255 ] 3 . The network assigns an RGB colour value to each point in space . In this work , the network weights are deterministic and point-estimated via maximum likelihood , the fully-Bayesian treatment of the colour map is left for future work . The prior and approximate posterior distributions over the occupancy map are : p ( Mocc ) = ∏ i , j , k N ( Mocci , j , k | 0 , 1 ) , qφ ( Mocc ) = ∏ i , j , k N ( Mocci , j , k | µi , j , k , σ2i , j , k ) . Here and for the rest of this work qφ will denote a variational approximate posterior distribution , with all its optimisable parameters summarised in φ . We assume p ( Mocc ) and qφ ( Mocc ) factorise over grid cells . The variational parameters µi , j , k , σi , j , k are optimised with Bayes by Backprop ( Blundell et al. , 2015 ) . Attention In the proposed model , the composition of the attention p ( mt | zt , M ) and the emission p ( xt | mt ) implements volumetric raycasting . We engineer them based on our understanding of geometry to ensure generalisation across unseen environments . The attention p ( mt | zt , M ) forms latent charts mt , which correspond to extracts from the map M around zt . We identify mt with the part of the map contained in the frustum of the current camera view . To attend to that region , first the intrinsic camera matrix K ( assumed to be known ) and the agent pose zt are used to cast a ray for any pixel [ i , j ] T in the reconstructed observation . The ray is then discretised equidistantly along the depth dimension into r-many points , resulting into a collection of 3D world coordinates pt ∈ Rw×h×r×3 . Depth candidate values d ∈ { k } 1≤k≤r are associated with each point along a ray , where is a resolution hyperparameter . The latent chart mt = ( ot , ct ) factorises into an occupancy chart ot ∈ Rw×h×r and a colour chart ct ∈ Rw×h×r×3 . Let pijkt ∈ R3 be a 3D point in the spanned camera frustum . To form the occupancy chart ot , cells from the map Mocc around pijkt are combined with a weighted kernel oijkt = ∑ l , h , sM occ l , h , sαl , h , s ( p ijk ) . Note that here l , h , s are indices of the occupancy map voxels . We choose a trilinear interpolation kernel for α , merging only eight map cells per point . This makes the attention fast and differentiable w.r.t zt . The colour chart ct = fMcol ( pt ) is formed by applying fMcol , the colour neural network , point-wise to each 3D point . In this work , we keep the chart mt deterministic . The full attention procedure can be described as : p ( mt | zt , M ) = ∏ ijk δ ( mijkt = fA ( M , pijk ) ) , pijk = T ( zt ) K−1 [ i , j , 1 ] T d︸︷︷︸ : =k . Here T ( zt ) ∈ SE ( 3 ) denotes the rigid camera transformation defined by the current agent state zt and i , j , k index the points lying inside the attended camera frustum . Emission through ray casting The emission model factorises over the observed pixels : p ( xt |mt ) = ∏ ij p ( xijt |mt ) , p ( xijt |mt ) = p ( dijt , c̃ijt | ot , ct ) . It operates on the extracted chart mt = ( ot , ct ) . Here x ij t ∈ R4 denotes an RGB-D pixel value , i.e . for each pixel [ i , j ] T we reconstruct a depth dijt and a colour value c̃ ij t . The mean of the depth value dijt is formed by a function fE : fE ( ot ) ij = · min k∈ [ r ] k s.t . oijkt > τ. fE traces the ray for pixel [ i , j ] T , searching for the minimum depth d = k for which the occupancy value oijkt exceeds a threshold τ ( a hyperparameter ) . 1 Since the above min operation is not differentiable in ot , we linearly interpolate between the depth value for the first ray hit and its predecessor to form the mean of the emitted depth ( cf . fig . 2b ) : µijdt = αfE ( ot ) ij + ( 1− α ) ( fE ( ot ) ij − ) , α = τ − oi , j , k−1t oi , j , kt − oi , j , k−1t . The mean of the emitted colour µijc̃t = c ijk t directly corresponds to the k-th element of the attended colour values , where k is the index of the first hit from raycasting above . A heteroscedastic Laplace distribution is assumed for both the emitted depth and colour values : p ( xijt |mt ) = Laplace ( xijt ; ( µijdt , µ ij c̃t ) , diag ( σijE ) ) . Agent states All agent states are represented as vectors zt = ( λt , ωt , zrestt ) ∈ Rdz . λt ∈ R3 is the agent location in space . ωt ∈ H4 is the agent orientation , represented as a quaternion . zrestt ∈ Rdz−7 is a remainder . Depending on the used transition model , zrestt can be λ̇t alone or it can contain an abstract latent portion not explicitly matching physical quantities . The approximate posterior variational family over the agent states factorises over time : qφ ( z1 : T ) = ∏ t qφ ( zt ) = ∏ t N ( zt | µzt , diag ( σzt ) 2 ) . Here µzt ∈ Rdz and σzt ∈ Rdz are free variables for each latent state and are optimised with SGVB ( Kingma & Welling , 2014 ) . Notably , the above factorisation over states bears similarity to pose-graph optimisation . One can see the individual terms qφ ( zt ) as graph nodes , and the loss terms induced by the transition and emission in the objective presented next as the edge constraints . Overall objective The elements described so far , together with the transition p ( zt+1 | zt , ut ) discussed in the next section , form the probabilistic graphical model in eq . ( 1 ) . The assumed variational approximate posterior is qφ ( z1 : T ) qφ ( M ) ≈ p ( z1 : T , M | x1 : T , u1 : T−1 ) . For the optimisation objective we use the negative evidence lower bound ( ELBO ) ( Jordan et al. , 1999 ) , given as Lelbo = −Eq [ T∑ t=1 log p ( xt |mt ) ] + KL ( qφ ( M ) || p ( M ) ) + Eq [ T∑ t=2 KL ( qφ ( zt ) || p ( zt | zt−1 , ut−1 ) ) ] . ( 2 ) We employ the approximate particle optimisation scheme from ( Mirchev et al. , 2019 ) to deal with long data sequences . The only optimised parameters are φ , containing the parameters of the map and the agent states . Making image reconstruction tractable Using the full observations during inference is not feasible , as raycasting for all pixels is too computationally demanding . To ensure tractability of the inference method we therefore use reconstruction sampling ( Dauphin et al. , 2011 ) , emitting a random part of xt at a time , by randomly selecting c-many pixel coordinates [ i , j ] T for every gradient step . Here c is a constant much smaller than the image size wh , speeding up gradient updates by a few orders of magnitude . Note that this results in an unbiased , faster and more memory-efficient Monte Carlo approximation of the original objective , avoiding loss of information due to subsampling or sparse feature selection . 1oijkt is set to 0 for k ≤ 1 and fE ( ot ) = r if no value exceeds τ along the ray .

Summary: This paper addresses the problem of dense RGB-D SLAM. The key idea is to formulate the problem as a deep state-space model and infer the state of the latent variables (i.e. pose and geometry) using variational inference. The experiments demonstrate that the method performs well in a challenging quadcopter dataset. However, the advantages of the approach are not demonstrated.