Direction-Aware Semi-Dense SLAM
Julian Straub

Randi Cabezas

John Leonard

John W. Fisher III

arXiv:1709.05774v1 [] 18 Sep 2017

Abstract
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.

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.

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

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.

1. Related Work
Among the wealth of recent 3D SLAM systems [13,
25, 33, 32, 49, 50, 29, 15, 12], there are only few who
1

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,

Sensor
RGBD
Camera Tracking

Obs. Extraction

T

Seg. z p, n of Surfels
Gibbs Sampler z, µ, τ
Statistics

Gibbs Sampler n
Gibbs Sampler p

Graph G

New
Surfels

Graph Builder

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. SalasMoreno 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.

2. 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 pi , orientation ni , color Ii and radius ri . For notational clarity let si = {pi , ni , Ii , ri } collect all properties
of surfel i. The SCW segmentation is expressed via surfel
labels {zi }. The world is observed via a RGB-D camera at
poses {Tt } where t indexes the pose at the reception of the
tth camera frame. From the RGBD image, we obtain point
observations xp , surface normal observations xn , and surface color Ic . We collect all observations of the tth frame in
the variable xt = {xn , xp , Ic }. Hence, the direction-aware
SLAM problem amounts to inference over the posterior:
p ({si }, {zi }, {Tt } | {xt })

dir.-aware SLAM .

(1)

camera pose:
T ? = arg minT ∈SE(3) fp2pl (T ) + λI fphoto (T )
P
fp2pl = i∈A σ21 knTi (T xpi − pi )k22
p2pl,i
P
1
fphoto = i∈A σ2 kIc (π(T T pi )) − Ii k22 ,

(5)
(6)
(7)

I

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:
p ({Tt } | {zi }, {si }, {xt })

dir.-aware loc ,

(2)

p ({si } | {zi }, {Tt }, {xt })

dir.-aware mapping ,

(3)

p ({zi } | {si }, {Tt }, {xt })

dir. segmentation .

(4)

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.

3. 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

where π(·) projects a 3D point into the image space. Note
that while we have use the familiar notation for surfel properties (here ni , pi ), 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:
X
Exp(ω))
ωk22 = kJω − bk22 , (8)
f=
kei (T ) + ∂ei (T ∂ω
i∈A

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

H(ω) ≥ 3 log(2πe) − 21 log J T J .
(9)
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.

Algorithm 1 Direction-aware incremental ICP.
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:

get observable surfels by rendering the map
while ICP not converged do
k=0
while uncertainty too large do
pick surfel with next lower k∇Ik2 in dir. seg. k
if plane passes occlusion reasoning then
add to ICP J T J and J T b; updated entropy
end if
k = (k + 1)%K
end while
compute transformation update
end while

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 Ni of neighboring surfels of surfel i.
From a generative standpoint, this model first samples
a countably infinite set of cluster weights πk , von-MisesFisher means µk , and concentrations τk from a Dirichlet
process with concentration α and base measure G0 :
(10)

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]:
 D/2−1 a

τ
p(µ, τ | µ0 , a, b) ∝
exp bτ µT µ0 , (11)
ID/2−1 (τ )
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 Ni ,
a label zi ∈ {1, . . . , ∞} is sampled to assign each surfel to
a von-Mises-Fisher distribution zi :
zi ∼ MRFz (zi , {zj }j∈Ni ; λ) Cat ({πk }∞
k=1 )

pi

ni

2
)
N (nTi pj ; nTi pi , σpl
pj

ni
pi

knTi (pj − pi )k2

(a) Point-to-Plane Cost

(b) Out-of-Plane Gaussian

Figure 4: Illustration of the point-to-plane cost function for
a surfel at location pi with normal ni to a point pj (left). A
probabilistic interpretation of this cost function is a Gaussian over the out-of-plane deviation of a point pj (right).
where λ is the weight of the MRF contribution and 1ab is 1
if a = b and 0 otherwise.

4. Directional Segmentation

{πk , µk , τk }∞
k=1 ∼ DP(α, G0 )

pj

(12)

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:
 P

MRFz = exp λ j∈Ni 1zzij − λ|Ni | ,
(13)

5. 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 (pi , ni , pj , nj , z) that encapsulates
local planarity is obtained by symmetrizing the well known
point-to-plane distance function used in implementations of
ICP [38]:
 zi


−1z
exp 2σ2j knTi (pj − pi )k22 + knTj (pi − pj )k22 . (14)
pl

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
2
variance σpl
over the out-of-plane deviation of the respective other surfel location. This geometry is shown in Fig. 4.

5.1. 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 Oi , we assume an iid Gaussian observation model
for locations {xpj }j∈Oi :
 Q
p {xpj }Oi | pi = j∈Oi N (xpj ; Tt−1 pi , Σp,j ) . (15)
The observation covariances Σp,j are computed according
to a realistic depth camera noise model [34] and incorporate
the linearized camera pose uncertainty:
Σp,j = ΣO,j + JT ΣT JTT .

(16)

The surfel orientation observations {xpj }j∈Oi are assumed to be iid von-Mises-Fisher distributed:
Q
p({xnj }Oi | ni ; τO ) = j∈Oi vMF(xnj ; RtT ni , τO ) , (17)
where we have used the inferred camera rotations
{Rt }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].

6. Sampling-based Inference over SCW Map
We now turn to describing how to perform posterior inference on the joint SCW map model given observations
{xp , xn , Ic } from inferred camera poses Tt . 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 ni Via Bayes’ law, the conditional
distribution of surfel direction ni , p(ni |{xnj }i , p, z), is proportional to
Q
pl
p(ni |µzi , τzi )p {xnj }i |ni
j∈Ni Ψij
Q
∝ vMF(ni ; µzi , τzi ) j∈Oi vMF(xnj , RtT ni , τO ) (18)
zi
Q
T
T
2 1zj
,
j∈Ni N (ni pi ; ni pj , σpl )
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 ni . 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
z

2 1zij
N (nTi pi ; nTi pj , σpl
)
∝ exp − 21 nTi Si ni ,

(19)

(a) Stds of [0.2, 0.2, 0.001]

(b) Stds of [0.1, 0.1, 0.001]

Figure 5: The approximation of a Bingham distribution
around one of its two modes with a von-Mises-Fisher is
close around the mode (visualization is in log scale).
z
P
1zi
where Si = j∈Ni σ2j (pi − pj )(pi − pj )T . This distribupl
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 Si with eigenvalues e1 < e2 < e3 and
associated eigenvectors q1 , q2 , q3 :


T
2 e2
exp(− 12 nTi Si ni ) ≈ exp e2e
q
n
,
(20)
i
2 +e3 1

which is proportional to a vMF distribution with mode q1
2 e2
. Figure 5 shows that the vMF apand concentration e2e
2 +e3
proximation is close to the Bingham distribution for several
realistic standard deviations of planar and slightly curved
surfaces. In practice, since Si 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(ni |{xnj }i , p, z)
over surface normal ni is indeed proportional to a vonMises-Fisher:
p({xnj }i |ni )p(ni |µ, τ, zi )p(ni |p, z) ∝ vMF(dϑe, kϑk2 )
P
2 e2
q1 ,
(21)
ϑ = j∈Oi xnj τO + µzi τzi + e2e
2 +e3
where xnj have been rotated into the world camera frame
ϑ
using the appropriate Rt and dϑe = kϑk
. An efficient
2
method for sampling from a von-Mises-Fisher distribution
is outlined in [47].
Sampling Directional Segmentation Labels zi 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:
 P

p(zi = k|zNi , µ, τ ) ∝ exp λ j∈Ni 1zzij − λ|Ni |
(22)

1
1zki Nk vMF(ni ; µk , τk ) + 1zK+1
αp(ni ; G0 )

where Nk 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 ni under the prior
on the vMF component distribution, p(ni ; G0 ), 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])

bτ exp bτ µT µ0

.
(23)
p(ni ; µ0 , a = 1, b) =
2π 2 tan bπ
2 sinh(τ )
Sampling vMF Parameters µk and τk Given sampled
normals ni assigned to von-Mises-Fisher clusters via labels
zi the posterior over the kth vMF mixture component mode
µk and concentration τk is:
Y
p(µk , τk |n, z; G0 ) ∝ p(µk , τk ; G0 )
p(ni |µk , τk )
i∈Ik
(24)
k
∝ p(µk , τk | µ̃0 , ãk , b̃k ) ,
wherePIk collects all surfels associated to cluster k. With
ϑ = i∈Ik ni + bµ0 , the posterior parameters ãk and b̃k
are computed as
ãk = a + |Ik | ,

b̃k = kϑk2 ,

µ̃k0 = dϑe .

(25)

Sampling Locations pi Conditioned on point observations {xpj }j∈Oi , and a surfel’s neighborhood Ni , a surfel’s
position is distributed as:
Y pl Y
p(pi |{xpj }i , n, p, z) ∝
Ψij
p(xpj |pi )
(26)
j∈Ni

j∈Oi

where the observation model p(xpj | pi ) is Gaussian as defined in Eq. (15). The MRF potential Ψpl
ij from Eq. (14) is
proportional to:

 zi
−1zj T
zi T
p
I
p
+
1
p
I
p
(27)
exp
i ij i
zj i ij j
2
where Iij =

1
T
2 (ni ni
σpl

+ nj nTj ) is the information matrix of

a degenerate Gaussian in information form and Iij pj is its
scaled mean. Since the individual distributions are all Gaussian the posterior over surfel location pi is also Gaussian [7]
with the following mean and variance:
p(pi | {xpi }, ni , p, z) ∝ N (pi ; µ̃i , Σ̃i )
P
P
zi
Σ̃−1
= j∈Oi Σ̃−1
i
p,j +
j∈Ni 1zj Iij
P
P
p
−1
zi
Σ̃−1
i µ̃i =
j∈Ni 1zj Iij pj .
j∈Oi Σ̃p,j Tt xj +

(28)
(29)
(30)

−1 T
where Σ̃−1
p,j = Rt Σp,j Rt . Note that there is always at least
one observation (i.e. |Oi | > 0) and therefore the inversion
to compute the variance is always determined.

6.1. 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(pi ) or p(ni ) 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 {ξjp }Si from the distribution of
surfel locations pi , we estimate the mean p̄i and variance
Σ̄i ofP
the surfel location using
P the accumulated statistics
p̃i = j∈Si ξjp and Opi = j∈Si ξjp ξjpT :
p̄i = Epi [pi ] ≈

p̃i
|Si | ,

Σ̄i = Var (pi ) ≈

Opi
|Si |

− p̃i p̃Ti . (31)

Note that samples ξjp are not samples from a Gaussian distribution but the maximum entropy distribution of pi 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 {ξjn }j∈Oi we compute
the mode n̄i of a vMF distribution for camera tracking using
the accumulated statistics ñi :
P
n̄i = dñi e ,
ñi = j∈Si ξjn .
(32)
To compute the most likely directional segment z̄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
zi . 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 the K̃ = 3 most likely cluster
assignments incrementally.

7. 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-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

Figure 6: Reconstruction (left), surfel segmentation (middle), and surface normal segmentation (right) of the fr2 xyz
dataset (top row) and an indoor area with couches (bottom row).
there are theoretical guarantees. In practice breaking the
samplers into parallel threads seems to make no difference.

8. Evaluation and Results

Figure 7: Reconstruction (left) and segmentation (right) of
an office including implicit loop closure.

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

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.
Qualitative Reconstruction Results 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

Figure 8: Thread timings (left) and surfel (middle) and sample count (right) statistics for the directional SLAM system
running on the fr2 xyz dataset [46].
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.

9. Conclusion
Figure 9: Comparison of the absolute trajectory error (ATE)
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

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.

References
[1] H. Badino, D. Huber, Y. Park, and T. Kanade. Fast and accurate computation of surface normals from range images. In
ICRA, pages 3084–3091. 2011. 5
[2] S. Y. Bao, M. Bagra, Y.-W. Chao, and S. Savarese. Semantic
structure from motion with points, regions, and objects. In
CVPR, pages 2703–2710. 2012. 2
[3] S. Y. Bao and S. Savarese. Semantic structure from motion.
In CVPR, pages 2025–2032. 2011. 2
[4] C. Bingham. An antipodally symmetric distribution on the
sphere. The Annals of Statistics, 2(6):1201–1225, 1974. 5
[5] D. Blackwell and J. B. MacQueen. Ferguson distributions
via pólya urn schemes. The Annals of Statistics, pages 353–
355, 1973. 5
[6] M. Bosse, R. Rikoski, J. Leonard, and S. Teller. Vanishing points and three-dimensional lines from omni-directional
video. The Visual Computer, 19(6):417–430, 2003. 1, 2

[7] P. Bromiley. Products and convolutions of Gaussian probability density functions. Technical Report Tina Memo No.
2003-003, University of Manchester. 6
[8] R. Cabezas, J. Straub, and J. W. Fisher III. SemanticallyAware Aerial Reconstruction from Multi-Modal Data. In
ICCV, 2015. 2
[9] G. Casella and E. George. Explaining the gibbs sampler. The
American Statistician, 46(3):167–174, 1992. 6
[10] R. O. Castle, D. Gawley, G. Klein, and D. W. Murray. Towards simultaneous recognition, localization and mapping
for hand-held and wearable cameras. In ICRA, 2007. 1, 2
[11] H. Cramér. Mathematical Methods of Statistics, volume 9.
Princeton University Press, 2016. 3
[12] A. Dai, M. Nießner, M. Zollhöfer, S. Izadi, and C. Theobalt.
BundleFusion: Real-time Globally Consistent 3D Reconstruction using On-the-fly Surface Re-integration TOG,
2017. 1
[13] A. J. Davison. Real-time simultaneous localisation and mapping with a single camera. In ICCV, 2003. 1
[14] F. Dellaert and R. Collins. Fast image-based tracking by selective pixel integration. In Proceedings of the ICCV Workshop on Frame-Rate Vision, 1999. 3
[15] J. Engel, T. Schöps, and D. Cremers. LSD-SLAM: Largescale direct monocular SLAM. In ECCV, 2014. 1, 3, 7
[16] N. Fioraio and L. Di Stefano. Joint detection, tracking and
mapping by semantic bundle adjustment. In CVPR, 2013. 2
[17] M. Habbecke and L. Kobbelt. A surface-growing approach
to multi-view stereo reconstruction. In CVPR, 2007. 2, 3
[18] A. Handa, T. Whelan, J. McDonald, and A. J. Davison. A
benchmark for RGB-D visual odometry, 3D reconstruction
and SLAM. In ICRA, 2014. 8
[19] M. Johnson, J. Saunderson, and A. Willsky. Analyzing hogwild parallel Gaussian Gibbs sampling. In NIPS, 2013. 7
[20] M. Kaess. Simultaneous localization and mapping with infinite planes. In ICRA, 2015. 1, 2
[21] M. Keller, D. Lefloch, M. Lambers, S. Izadi, T. Weyrich, and
A. Kolb. Real-time 3D reconstruction in dynamic scenes
using point-based fusion. In International Conference on
3DTV-Conference, 2013. 2, 3
[22] C. Kerl, J. Sturm, and D. Cremers. Dense visual SLAM for
RGB-D cameras. In IROS, 2013. 3
[23] C. Kerl, J. Sturm, and D. Cremers. Robust odometry estimation for RGB-D cameras. In ICRA, 2013. 3
[24] B.-s. Kim, P. Kohli, and S. Savarese. 3D scene understanding
by voxel-CRF. In ICCV, 2013. 2
[25] G. Klein and D. Murray. Parallel tracking and mapping on a
camera phone. In ISMAR, 2009. 1
[26] A. Kundu, Y. Li, F. Dellaert, F. Li, and J. M. Rehg. Joint semantic segmentation and 3D reconstruction from monocular
video. In ECCV, 2014. 2
[27] S. Lovegrove and A. J. Davison. Real-time spherical mosaicing using whole image alignment. In ECCV, 2010. 6
[28] L. Ma, C. Kerl, J. Stueckler, and D. Cremers. CPA-SLAM:
Consistent plane-model alignment for direct RGB-D SLAM.
In ICRA, 2016. 1, 2

[29] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos. ORBSLAM: a versatile and accurate monocular SLAM system.
Transactions on Robotics, 31(5):1147–1163, 2015. 1
[30] P. K. Nathan Silberman, Derek Hoiem and R. Fergus. Indoor
segmentation and support inference from RGBD images. In
ECCV, 2012. 3
[31] R. Neal. Markov chain sampling methods for Dirichlet process mixture models. Journal of computational and graphical statistics, 9(2):249–265, 2000. 5
[32] R. A. Newcombe, A. J. Davison, S. Izadi, P. Kohli,
O. Hilliges, J. Shotton, D. Molyneaux, S. Hodges, D. Kim,
and A. Fitzgibbon. Kinectfusion: Real-time dense surface
mapping and tracking. In ISMAR, 2011. 1, 4
[33] R. A. Newcombe, S. J. Lovegrove, and A. J. Davison.
DTAM: Dense tracking and mapping in real-time. In ICCV,
2011. 1
[34] C. V. Nguyen, S. Izadi, and D. Lovell. Modeling Kinect
sensor noise for improved 3D reconstruction and tracking.
In 3DIMPVT, 2012. 4, 5
[35] G. Nunez-Antonio and E. Gutiérrez-Pena. A Bayesian
analysis of directional data using the von Mises-Fisher
distribution. Communications in StatisticsSimulation and
Computation R , 34(4):989–999, 2005. 4
[36] P. Orbanz and J. Buhmann. Smooth image segmentation by
nonparametric Bayesian inference. ECCV, 2006. 4
[37] B. Peasley, S. Birchfield, A. Cunningham, and F. Dellaert.
Accurate on-line 3D occupancy grids using Manhattan world
constraints. In IROS, 2012. 1, 2
[38] S. Rusinkiewicz and M. Levoy. Efficient variants of the ICP
algorithm. In International Conference on 3-D Digital Imaging and Modeling, 2001. 4
[39] R. F. Salas-Moreno, B. Glocken, P. H. Kelly, and A. J. Davison. Dense planar SLAM. In ISMAR, 2014. 1, 2
[40] R. F. Salas-Moreno, R. A. Newcombe, H. Strasdat, P. H.
Kelly, and A. J. Davison. SLAM++: Simultaneous localisation and mapping at the level of objects. In CVPR, 2013.
2
[41] A. Segal, D. Haehnel, and S. Thrun. Generalized-ICP. In
RSS, 2009. 3
[42] J. Straub. Nonparametric Directional Perception. PhD thesis, Massachusetts Institute of Technology, 2017. 6
[43] J. Straub, T. Campbell, J. P. How, and J. W. Fisher III. Smallvariance nonparametric clustering on the hypersphere. In
CVPR, 2015. 1, 4
[44] J. Straub, J. Chang, O. Freifeld, and J. W. Fisher III. A dirichlet process mixture model for spherical data. In AISTATS,
2015. 1, 4
[45] J. Straub, G. Rosman, O. Freifeld, J. J. Leonard, and J. W.
Fisher III. The Manhattan frame model – Manhattan world
inference in the space of surface normals. In TPAMI, 2017.
1, 4
[46] J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers. A benchmark for the evaluation of RGB-D SLAM
systems. In IROS, 2012. 8
[47] G. Ulrich. Computer generation of distributions on the msphere. Applied Statistics, pages 158–163, 1984. 5

[48] T. Weise, T. Wismer, B. Leibe, and L. Van Gool. In-hand
scanning with online loop closure. In ICCV Workshops,
2009. 2, 3
[49] T. Whelan, M. Kaess, M. Fallon, H. Johannsson, J. Leonard,
and J. McDonald. Kintinuous: Spatially extended kinectfusion. 2012. 1
[50] T. Whelan, R. F. Salas-Moreno, B. Glocker, A. J. Davison,
and S. Leutenegger. Elasticfusion: Real-time dense SLAM
and light source estimation. IJRR, pages 1697–1716, 2016.
1, 3
[51] J. Xiao, A. Owens, and A. Torralba. Sun3D: A database of
big spaces reconstructed using SFM and object labels. In
ICCV, 2013. 2

