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 presents a novel learning-based visual-inertial odometry algorithm. The algorithm simultaneously reconstructs the world map as well as the states of the agent from the stereo RGBD sensors.  The world is modeled as an occupancy grid with color. A graphical model with attention mechanism and ray casting is used to model how the world and the agent state renders the RGBD sensor data. ELBO is used to optimize the model. The technical details look sound to me.