{'fraction_non_alphanumeric': 0.03082679971489665, 'fraction_numerical': 0.010584461867426943, 'mean_word_length': 4.68381608264128, 'pattern_counts': {'":': 0, '<': 6, '<?xml version=': 0, '>': 1, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 5, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Direction-Aware Semi-Dense SLAM

To aide simultaneous localization and mapping (SLAM), future perception systems will incorporate forms of scene understanding. In a step towards fully integrated probabilistic geometric scene understanding, localization and mapping we propose the first direction-aware semi-dense SLAM system. It jointly infers the directional Stata Center World (SCW) segmentation and a surfel-based semi-dense map while performing real-time camera tracking. The joint SCW map model connects a scene-wide Bayesian nonparametric Dirichlet Process von-Mises-Fisher mixture model (DP-vMF) prior on surfel orientations with the local surfel locations via a conditional random field (CRF). Camera tracking leverages the SCW segmentation to improve efficiency via guided observation selection. Results demonstrate improved SLAM accuracy and tracking efficiency at state of the art performance.

Future perception systems in applications such as autonomous cars, autonomous robots, or augmented reality will integrate scene understanding into the purely geometric localization and mapping task. This is likely to improve both simultaneous localization and mapping (SLAM), provide a basis for higher-level reasoning about the scene, and richer information for human operators. In current systems scene understanding is used in two ways: (1) to improve the operation of the 3D perception system and (2) to provide additional information for higher-level inference or a human operator. We argue that only systems in the first class actually "understand" aspects of the scene because they are able to use inferred concepts to improve on their other inferential tasks (i.e. localization and mapping). The number of systems that fall into this class is still small.
In order to improve 3D reconstruction and localization via scene understanding most approaches rely on geometric scene priors such as planarity [10,39,20,28], the Manhattan World [37] (MW) or the Stata Center World [6] (SCW) assumption. The assumption of planarity is a local assumption that cannot explain the connection between disparate scene parts like all the parallel planes in typical man-made environments. Such connections can be captured and explained by global assumptions such as the MW and the SCW assumption. Because the MW assumption is limited Figure 1: We propose the first direction-aware semi-dense SLAM system. Based on the Stata Center World assumption the system jointly infers a directional segmentation (right) and a semi-dense surfel-based map (left) in real-time.
to very specific environments, we instead explore the flexible Stata Center World model to improve 3D reconstruction and camera tracking. As shown in [43,44], the directional clustering of a scene's surface normals under the Stata Center World implies a segmentation that captures scene-wide regularities of the environment [45] as can be seen in the segmentation in Fig. 1.
Based on the SCW scene prior, we propose the first semi-dense nonparametric direction-aware SLAM system. It performs joint inference over the Bayesian nonparametric SCW scene segmentation and the world map using Gibbssampling without precluding real-time operation. To connect the scene-wide Stata Center World model with local surface properties we model the assumption that nearby areas in the same directional segment are likely planar using a CRF. We demonstrate experimentally that using the directional segmentation improves SLAM accuracy and camera tracking efficiency via guided observation selection.

Related Work
Among the wealth of recent 3D SLAM systems [13,25,33,32,49,50,29,15,12], there are only few who jointly reason about 3D structure, geometric segmentation, and camera trajectory. The most common geometric prior is planarity of the environment. Castle et al. [10] are among the visual SLAM systems to incorporate planar geometry. They augment their visual SLAM system with the ability to detect known planar patches and use them to improve SLAM. Salas-Moreno et al. [39] integrate plane segmentation into the tracking and reconstruction pipeline of a dense surfel-based reconstruction system. Results show that utilizing a plane segmentation of the environment leads to improved tracking accuracy. Kaess [20] explores a plane-based SLAM formulation wherein the map directly consists of infinite planes which are being jointly optimized with the camera pose in a smoothing and mapping (SAM). Ma et al. [28] demonstrate joint inference over a key-frame-based map and a plane segmentation of the environment. The joint formulation with soft planeassignments reduces drift of the SLAM system. In comparison to the plane-based approaches, the proposed system does not need to explicitly extract planes and imposes scene-wide constraints as opposed to local constraints. Furthermore sampling-based inference allows soft associations to directions that can be refined and corrected whereas all but the EM-based CPA-SLAM [28] make hard assignments to specific planes that are not revisited [10,39,20]. More related to the proposed SCW-based approach is the system by Peasley et al. [37] who use the Manhattan World assumption to impose global constraints on the 2D trajectory of a robot. They show that this yields drift-free SLAM, eliminating the need for loop-closures, given that the MW assumption holds. Bosse et al. [6] essentially use the SCW assumption in the image space via vanishing point (VP) detection. They incorporate VP tracking into a SLAM system to jointly estimate a robot's trajectory and the sparse 3D location of lines in the environment.
Beyond the aforementioned planar, MW and SCW assumptions several approaches have been proposed that incorporate human-annotated semantic labels and shapes into SLAM. Bao et al. [3,2] jointly estimate object and region segmentation of a sparse point cloud in the batch structure from motion framework. Similarly, Fioraio et al. [16] jointly perform incremental object detection, mapping and camera pose estimation in what they call semantic bundle adjustment. Xiao et al. [51] show how enforcing semantic label consistency in a 3D reconstruction system leads to better 3D reconstruction by decreasing drift and correcting loop closures. Kundun et al. [26] jointly use dense image segmentation and the raw RGB image captured from a single camera to infer the camera trajectory and, using a conditional random field (CRF) defined over an occupancy grid, a semantic 3D reconstruction. Working in the realm of RGBD cameras as well, Kim et al. [24] use a voxelbased world representation and, for a given RGBD image, Graph G Figure 2: Architecture of the direction-aware 3D reconstruction system. Boxes denote algorithm components, and dotted boxes designate the five different threads that are running in parallel. Arrows are marked with the output produced by an algorithm block and consumed by another.
infer the 3D occupancy (i.e. the 3D structure) and the segmentation of the environment into semantic classes. Salas-Moreno et al. [40] are the first to demonstrate a SLAM system that utilizes dense 3D object models as beacons for camera tracking and map representation. Closer in spirit to our approach, Cabezas et al. [8] use a mixture-model over scene features (appearance, surface normals and semantic observations) as a prior-probability model to discover and encourage scene-wide structure. They show that the learned scene-specific priors improve the 3D reconstruction. In comparison, our method not only discovers scenewide structure but also connects the scene-wide model to the local 3D reconstruction.

Direction-Aware Semi-Dense SLAM
We define direction-aware SLAM as reasoning about the joint distribution of a world map m, the trajectory of the perception system and the directional segmentation z given observations x. Concretely, we represent the map as a set of surfels [21,48,17]. Surfels are localized planes with position p i , orientation n i , color I i and radius r i . For notational clarity let s i = {p i , n i , I i , r i } collect all properties of surfel i. The SCW segmentation is expressed via surfel labels {z i }. The world is observed via a RGB-D camera at poses {T t } where t indexes the pose at the reception of the tth camera frame. From the RGBD image, we obtain point observations x p , surface normal observations x n , and surface color I c . We collect all observations of the tth frame in the variable x t = {x n , x p , I c }. Hence, the direction-aware SLAM problem amounts to inference over the posterior: Figure 3: Percentage of inlier scene-points as described by a randomly sampled set of planes as a function of the number of planes. The plots summarize statistics over all of the scenes in the NYU v2 dataset [30].
We perform inference on this direction-aware SLAM posterior by interleaving inference about the three subproblems of localization, mapping and directional segmentation: To accommodate operation at camera frame-rate the inference is split into two main parts: (1) real-time maximum likelihood camera pose estimation (Eq. (2)) and (2) sampling-based joined inference on segmentation and map (Eq. (3) and (4)) which runs in the background. An overview of the direction-aware SLAM system is depicted in Fig. 2.
Instead of aiming to represent all surfaces in the environment densely, as in related work [21,48,17], we sample the surfaces of the environment sparsely with a bias towards high intensity gradient areas for two reasons: (1) a sparse sampling of environment surface captures the majority of surfaces and scene structure, (2) a bias towards high intensity gradient areas captures visually salient regions for camera tracking [15]. To substantiate the first point we show the percentage of inlier scene-points to a randomly sampled set of planes as a function of the number of planes in Fig. 3 across all 1449 scenes of the NYU v2 dataset [30]. As little as 50 planes are enough to describe an average of 60% of the scene.

Direction-aware Camera Pose Estimation
For each observed RGBD frame we run the iterative closest point (ICP) algorithm to find a local optimum of the camera pose as well as data association between the observations and the global map surfels. The optimization of the camera pose given a projective data association A amounts to maximizing the negative log likelihood of the camera pose: where π(·) projects a 3D point into the image space. Note that while we have use the familiar notation for surfel properties (here n i , p i ), in practice sample-based estimates, computed as described in Sec. 6, are used. This cost function combines a point-to-plane (p2pl) and a photometric (photo) cost as employed by [50]. The probabilistic interpretation was developed in [41] and an extension to include a photometric term is straight forward [23]. The common strategy to obtain a camera pose estimate given data association is to Taylor-expand the error terms around the current transformation estimate: where we have collected the individual derivatives and error terms into the rows of J and b respectively. The variable ω ∈ se (3) is a small perturbation of the transformation in the tangent space to SE(3) at the current estimate of the transformation T . The least-squares solution for the (small) motion ω along the manifold SE (3) is obtained via the standard pseudo inverse (J T J) −1 J T b . As noted previously by Kerl et al. [22] the term J T J is the Fisher information matrix of the estimator. The variance of the estimate can be lower-bound by the Fisher information matrix using the Cramer-Rao bound [11]. Therefore, the entropy of the estimate ω is lower-bound via The task of the perception system is to improve the lowerbound on the true variance and entropy to enable more certain estimates. Our variant of ICP, outlined in Algorithm 1, incrementally adds planes to the cost function until low enough entropy is reached. Planes are chosen in a round-robin style from each of the Stata Center World segments in order of decreasing surfel texture gradient strength. Intuitively a diverse set of observed plane orientations provides better constrain the point-to-plane cost function (at least three differently orientated planes have to be observed to constrain the system fully). Preference for high gradient image regions is important for the photometric part of the ICP cost function. Similar to the approach by Dellaert et al. [14], the proposed ICP variant selectively integrates informative observations which decreases the number of necessary observations in practice and thus speeds up camera tracking.

Directional Segmentation
Under the Stata Center World model we make the assumption that the surface normal distribution of surfels has characteristic, low-entropy patterns as leveraged in related work by Straub et al. [45,43,44]. Similar to [43], we capture the notion of the Stata Center World model, that the surfel surface normal distribution consists of some variable, unknown number of clusters by a Dirichlet process von-Mises-Fisher mixture model. Following the proposal of [36], we impose spatial smoothness of the Stata Center World segmentation by assuming a Markov random field (MRF) over the segmentation z that encourages uniform labeling inside a set N i of neighboring surfels of surfel i.
From a generative standpoint, this model first samples a countably infinite set of cluster weights π k , von-Mises-Fisher means µ k , and concentrations τ k from a Dirichlet process with concentration α and base measure G 0 : To define the base measure, we utilize the conjugate prior for the von-Mises-Fisher distribution which in general is only known up to proportionality [35]: where 0 < b < a. The parameters of the prior are the directional mode µ 0 and a and b where a can be understood as pseudo-counts and b as the concentration mode. Second, given the cluster weights π k and the local neighborhood N i , a label z i ∈ {1, . . . , ∞} is sampled to assign each surfel to a von-Mises-Fisher distribution z i : The MRF smoothness component in practice helps speed up inference and leads to more uniform segmentations in the face of noise. It takes the form: where λ is the weight of the MRF contribution and 1 a b is 1 if a = b and 0 otherwise.

Direction-aware Mapping
We use another Markov random field over neighboring surfels to express a local planarity assumption over points in the same directional segment. The MRF connects the scenewide directional segmentation with local spatial properties. The MRF potential Ψ pl ij (p i , n i , p j , n j , z) that encapsulates local planarity is obtained by symmetrizing the well known point-to-plane distance function used in implementations of ICP [38]: While the point-to-plane cost function penalizes the outof-plane deviation of a point, the MRF potential employed herein can be seen as the product of two Gaussians with variance σ 2 pl over the out-of-plane deviation of the respective other surfel location. This geometry is shown in Fig. 4.

Observation Models for Mapping
Surfel locations and orientations are observed via the camera located at the estimated pose T . Associations between RGB-D observations and map surfels are established using projective data association [32]. Back-facing and occluded surfels are pruned. Occlusion is detected if a surfel observation has low probability. Capturing the cameraframe times at which observations of surfel i were taken in the set O i , we assume an iid Gaussian observation model for locations {x p j } j∈Oi : The observation covariances Σ p,j are computed according to a realistic depth camera noise model [34] and incorporate the linearized camera pose uncertainty: The surfel orientation observations {x p j } j∈Oi are assumed to be iid von-Mises-Fisher distributed: where we have used the inferred camera rotations {R t } j∈Oi . Surface normals are extracted using the fast yet robust unconstrained scatter-matrix approach by Badino et al. [1]. It is unclear how camera pose noise and depth image noise influences the surface normal concentration. Hence, we use a conservative observation concentration of τ O = 100 which makes the realistic assumption that 99% of the observed surface normals lie within a solid angle of about 18 • around the true surface normal. A more detailed model could be obtained with a controlled experiment similar to [34].

Sampling-based Inference over SCW Map
We now turn to describing how to perform posterior inference on the joint SCW map model given observations {x p , x n , I c } from inferred camera poses T t . Because the directional segmentation involves a Bayesian nonparametric Dirichlet process prior, we rely on Gibbs sampling inference, which in the limit of sampling guarantees samples from the true posterior distribution. The Gibbs sampler iterates sampling from the different conditional distributions of each random variable in the join SCW map model. In the following we provide details on sampling from each conditional distributions before detailing how samples are used to inform camera tracking.
Sampling Normals n i Via Bayes' law, the conditional distribution of surfel direction n i , p(n i |{x n j } i , p, z), is proportional to where we have abbreviated {·} j∈Oi with {·} i and used that only one of the two out-of-plane Gaussians in the MRF depends on n i . The first factor stems from the directional Stata Center World mixture model, and the second from the surface normal observation model. To sample from this distribution we derive a close approximation to the out-ofplane Gaussian that has the form of a vMF distribution. This makes the posterior over surface normals von-Mises-Fisher distributed which can be sampled efficiently. The Gaussian distribution on out-of-plane deviations of neighboring points can be re-arranged as where tion has the form of a Bingham distribution [4]. To keep in the realm of the von-Mises-Fisher distribution, we approximate this Bingham with a vMF distribution using the eigen decomposition of S i with eigenvalues e 1 < e 2 < e 3 and associated eigenvectors q 1 , q 2 , q 3 : which is proportional to a vMF distribution with mode q 1 and concentration 2e2e2 e2+e3 . Figure 5 shows that the vMF approximation is close to the Bingham distribution for several realistic standard deviations of planar and slightly curved surfaces. In practice, since S i incorporates only neighbors in the same directional segment (which are therefore likely to lie roughly in the same plane), we find the approximation to work well.
Under this approximation the posterior p(n i |{x n j } i , p, z) over surface normal n i is indeed proportional to a von-Mises-Fisher: where x n j have been rotated into the world camera frame using the appropriate R t and ϑ = ϑ ϑ 2 . An efficient method for sampling from a von-Mises-Fisher distribution is outlined in [47].
Sampling Directional Segmentation Labels z i We use the Chinese restaurant process (CRP) representation of the Dirichlet process [5,31] since it lends itself to straightforward sampling-based inference. The posterior for the directional segmentation label of surfel i is: where N k is the number of surfels associated to cluster k excepting the ith surfel, α is the Dirichlet process concentration and λ is the weight of the MRF contribution. The marginal distribution of surface normal n i under the prior on the vMF component distribution, p(n i ; G 0 ), can be derived in closed form for the vMF prior parameters a = 1 and 0 < b < 1 in D = 3 dimensions (see Sec. 2.6.3 [42]) Sampling vMF Parameters µ k and τ k Given sampled normals n i assigned to von-Mises-Fisher clusters via labels z i the posterior over the kth vMF mixture component mode µ k and concentration τ k is: where I k collects all surfels associated to cluster k. With ϑ = i∈I k n i + bµ 0 , the posterior parametersã k andb k are computed as Sampling Locations p i Conditioned on point observations {x p j } j∈Oi , and a surfel's neighborhood N i , a surfel's position is distributed as: where the observation model p(x p j | p i ) is Gaussian as defined in Eq. (15). The MRF potential Ψ pl ij from Eq. (14) is proportional to: where is the information matrix of a degenerate Gaussian in information form and I ij p j is its scaled mean. Since the individual distributions are all Gaussian the posterior over surfel location p i is also Gaussian [7] with the following mean and variance: Note that there is always at least one observation (i.e. |O i | > 0) and therefore the inversion to compute the variance is always determined.

Estimates Computed from the Samples
We use the Gibbs-sampler samples to approximately compute means and variances of surfel locations and orientations. Via the law of large numbers and by the construction of the Gibbs sampler this approach will in the limit converge to the true means and variances [9]. In practice, since the marginal distributions p(p i ) or p(n i ) are mostly concentrated about a single mode, the estimates converge quickly. In our experiments in the order of ten samples were sufficient to get usable estimates for real-time camera tracking as described in Sec. 3.
Given a set of samples {ξ p j } Si from the distribution of surfel locations p i , we estimate the meanp i and variancē Σ i of the surfel location using the accumulated statistics p i = j∈Si ξ p j and O pi = j∈Si ξ p j ξ pT j : Note that samples ξ p j are not samples from a Gaussian distribution but the maximum entropy distribution of p i is a Gaussian with the aforementioned mean and variance. The entropy of this Gaussian is an upper bound on the true entropy of the surfel location distribution and can serve as a scalar indicator of the uncertainty.
From the surfel normal samples {ξ n j } j∈Oi we compute the moden i of a vMF distribution for camera tracking using the accumulated statisticsñ i : To compute the most likely directional segmentz i of surfel i we would ideally keep a count of the number of times the surfel is assigned to each directional cluster via label z i . Since the number of clusters keeps growing and we aim for this estimation to be efficient for large numbers of surfels, we only keep track of theK = 3 most likely cluster assignments incrementally.

Implementation
In practice, to use the proposed approach we architect a multi-threaded system as depicted in Fig. 2. The main five threads are (1) a real-time data acquisition, camera tracking and observation extraction thread, (2) a nearest neighborhood graph builder thread and (3)(4)(5) three Gibbs sampler threads. Camera tracking utilizes RGBD frames and the current most likely estimate of the segmentation and surfel map to infer the current camera pose T . To be able to deal with fast motions we perform photometric rotational pre-alignment [27] from image pyramid level 3 down to 1.
For the same reason, we run direction-aware ICP from scale pyramid levels 1 down to 0. The observation extraction algorithm adds new surfels by uniformly sampling so-far unobserved surfaces with a bias towards high gradient surface  areas similar to [15]. The graph builder thread uses the initial locations of all surfels to maintain a k-nearest-neighbor graph over surfels (here k = 12) using the negative log MRF potential from Eq. (14) as the distance function. Valid neighbors have to be within a Euclidean radius of 0.2 m. This is an approximation to the directed graph that could be obtained by connecting all surfels within some distance. Retaining only the top k closest (under the potential) surfels improves algorithm efficiency without notable differences in the reconstruction results. To deal with deleted and newly added surfels, the thread additionally randomly revisits and potentially updates the nearest neighbors of already incorporated surfels. We split the Gibbs sampler into three threads each sampling (at its own speed) from the respective posterior given samples from the other threads. There exists only preliminary research on parallel Gibbs sampling under the name Hogwild Gibbs sampling [19] and it is unclear if there are theoretical guarantees. In practice breaking the samplers into parallel threads seems to make no difference.

Evaluation and Results
In the following we evaluate the proposed directionaware 3D reconstruction system on various challenging datasets quantitatively as well as qualitatively. All experiments are performed on a machine with an Intel Xeon CPU with 16 cores at 2.4 GHz and a Nvidia GTX-1080 graphics card. As described in Sec. 7, the algorithm utilizes a total of 5 CPU cores for the main inference tasks. Surface normals are computed only sparsely on CPU wherever needed. The GPU is used for the full-frame operations of SO(3) prealignment and data preprocessing. In Figures 1, 6, and 7 we show the RGB-colored and the SCW segmented 3D reconstructions of different scenes. As can be seen, the maximum a-posteriori estimate of the Stata Center World segmentation sensibly partitions the environment according to the surfel directions. The inference extracts the main peaks of the distribution which correspond to planar regions in the scene. Additionally, low concentration clusters are inferred that capture noisy, non-planar regions (green in top and yellow in bottom row of Fig. 6, yellow in Fig. 7).

Algorithm Operation and Properties
To explore the properties of the algorithm, we discuss timings, surfel and sampling statistics collected during the reconstruction of the  in meters as defined in [46] of different SLAM systems for different synthetic datasets from the benchmark dataset by Handa et al. [18] (kt0-2) and the TUM indoor dataset [46]. Note that all other systems utilize loop-closure techniques to correct for drift and tracking errors. fr2 xyz dataset [46] displayed in Fig. 6. Figure 8 (left) shows that the main camera tracking thread mostly runs in less than 50ms per frame. Runtime increases when the camera moves far away from the scene and ICP processes more points for confident camera tracking (see Fig. 8 middle). The runtime of the surfel parameter sampling threads scales with the size of the map (compare Fig. 8 middle). As can be seen in Fig. 8 (middle), the number of surfels utilized for camera tracking is usually less than 1000 surfels even if a magnitude more surfels are in view. This is enabled by the direction and gradient-aware selection of surfel observations. The statistics in Fig. 8 (right) show that while the number of surfels in the map keeps growing, the sampling threads yield sufficient samples per surfel.
Camera Tracking Accuracy Comparison We use the TUM indoor dataset [46] and the synthetic dataset by Handa et al. [18] to evaluate the camera tracking accuracy against groundtruth via the absolute trajectory error (ATE) [46] and compare our system to related 3D SLAM systems in Fig. 9. Fig. 9 demonstrates that the proposed directional SLAM system is on par or better than related algorithms in terms of camera trajectory estimation for datasets without the need for loop closures. The Dir. SLAM Random system uses direct surfel fusion and randomly selects ICP observations. As can be seen, disregarding the directional segmentation decreases tracking accuracy especially on the real datasets fr2 xyz and fr2 desk.

Conclusion
We have introduced the first direction-aware semi-dense SLAM system which performs joint inference over directional segmentation, surfel-based map and camera pose. Its direction-awareness manifests in that it can utilize the directional segmentation for its other tasks. The use of Gibbssampling-based inference on the complex Bayesian nonparametric segmentation and map model in a real-time reconstruction system has not been demonstrated before. Due to the flexibility of Gibbs-sampling this opens up exciting possibilities for inference on more complex and detailed environment models. Having access to samples from the posterior also allows reasoning about uncertainty which is not possible with the commonly employed mode-seeking inference methods.