A Unifying View of Geometry, Semantics, and Data Association in SLAM

1 Introduction

This paper bridges the gap between the advances in SLAM, relying on purely geometric information, and those in visual recognition, relying on purely semantic information. We present an abridged version of [Bowman et al., 2017] but also state key results leading to a unified view of geometry, semantics, and data association in SLAM. SLAM is the problem of estimating the motion of a sensor system, while simultaneously building a map of the environment, using a dense occupancy representation or a sparse set of landmarks (e.g., corner, edge, or plane features). A comprehensive survey can be found in [Cadena et al., 2016]. The problem is closely related to structure from motion (SfM) [Ma et al., 2012] with a moot distinction that SfM is typically an offline process relying on camera measurements, while SLAM is an online operation using heterogeneous sensors found on mobile robots, including inertial measurement unit (IMU), camera, and LIDAR. The problem is also related to visual-inertial odometry (VIO) [Mourikis and Roumeliotis, 2006], which uses geometric features to infer the sensor's motion but does not construct a global map. SLAM approaches went through several transformations, starting with particle and Kalman filtering [Thrun et al., 2005] and converging to iterative nonlinear optimization over the whole sensor trajectory [Strasdat et al., 2010]. Current approaches model the problem as a (factor) graph of sensor and landmark states connected via edges capturing measurement constraints [K¨ummerle et al., 2011; Kaess et al., 2012]. They can track visual-inertial systems over long trajectories in real time [Bloesch et al., 2015; Forster et al., 2016; Qin et al., 2017], while recovering the sparse [Engel et al., 2014; Mur-Artal and Tard´os, 2017] or dense [Whelan et al., 2016; Hornung et al., 2013] environment structure. Surprisingly, most methods rely only on geometry and occurred in isolation from the impressive results in object recognition based on structured models [Felzenszwalb et al., 2010] and deep neural networks [Krizhevsky et al., 2012; Girshick et al., 2014; Simonyan and Zisserman, 2014; He et al., 2016]. Our goal is to provide a meaningful and efficient environment representation in SLAM by taking advantage of object recognition methods. A recent survey on semantic mapping can be found in [Kostavelis and Gasteratos, 2015]. The works closest to ours [Bao and Savarese, 2011; Salas-Moreno et al., 2013; G´alvez-L´opez et al., 2016; Murthy et al., 2017] overlook the data association problem (matching observations to correct landmarks in the map) which is critical for reconstructing a global object map with accurate loop closures (recognizing already visited locations). Data association is challenging when many areas look alike (perceptual aliasing) and a single wrong choice can drastically affect the estimation accuracy. We make the following contributions.

∗ Supported by ARL DCIST CRA W911NF-17-2-0181.

Figure 3: Structured object model with prior shape constraints (purple lines) and online camera constraints (red lines).

Figure 4: Factor graph (right) of two sensor states xt, xt+1 constrained by IMU measurement it+1 and semantic observations St, St+1 (green) of landmarks li.

• We generalize the geometric environment model of SLAM by introducing structured object models of midlevel semantic landmarks (Fig. 1).
• Instead of making hard, potentially wrong associations among observations and landmarks, we show that SLAM inference can be done efficiently using a probability distribution over all possible associations, leading to significant advantages in ambiguous environments.

2 Metric-Semantic SLAM

2.1 Representation

Consider a sensor system consisting of an IMU and monocular camera that are time synchronized. Choose a subset of the camera images as keyframes (e.g., by selecting every nth frame) and let xt be the system state (6-D pose, velocity, and IMU biases) corresponding to the tth keyframe. Let X := {xt}Tt=0 be the sensor state trajectory and let the environment be represented as a collection of landmarks L := {lm}Mm=1 with positions lm ∈ R3 . We distinguish between geometric landmarks λj estimated from geometric features Yt (e.g., Harris corners, SIFT, ORB, etc.) and semantic landmarks lm estimated from semantic features St (parts of detected objects) extracted from each keyframe image as shown in Fig. 2. Geometric Structure. The IMU provides a set It+1 of linear acceleration and rotational velocity measurements between keyframes t and t + 1. These measurements are integrated using the approach of [Forster et al., 2015] to obtain a single constraint that relates xt and xt+1 via an IMU mea-

surement model f with noise covariance U:

The model in (1) relates two system states and specifies a Gaussian density function p(it+1|xt, xt+1) for the IMU measurement it+1. Similarly, each geometric feature ytk ∈ Yt for k = 1, . . . , Kt extracted at time t relates the position λj ∈ R3 of a corresponding geometric landmark to the system state xt via a camera projection model g [Ma et al., 2012, Ch.3]:

To recover the landmark positions {λj}Jj=1 , it is necessary to know the data association πt(k) = j of each feature ytk to the correct landmark λj. Estimating πt : {1, . . . ,Kt} → {1, . . . , J} can done by tracking the features ytk in consecutive keyframes. For example, we extract ORB features and track them by minimizing the ORB descriptor distance between consecutive frames [Rublee et al., 2011]. As discussed in Sec. 2.2, this visual odometry allows us to infer the sensor trajectory X locally but is not sufficient for loop closure and estimation of the global landmark positions {λj}.

Semantic Structure. Instead of using geometric landmarks to model the environment, our first innovation is to extract semantic features (object parts) and represent the environment as a set of objects, with inferred positions, orientations, and classes (Fig. 1). We assume a known list of object classes (with prior training data) and detect bounding boxes in each keyframe via a real-time detector such as DPM [Dubout and Fleuret, 2013], RCNN [Ren et al., 2015], or YOLO [Redmon and Farhadi, 2016]. Within each bounding box, we extract semantic features specific to the object class via the stacked hourglass convolutional network of [Pavlakos et al., 2017]. Let St be the set of all |St| = Nt semantic features extracted from the tth keyframe. Similar to the geometric case, each semantic feature stn ∈ St is related to the position lm ∈ R3 of a semantic landmark via the camera perspective projection:

and an unknown function dt : {1, . . . ,Nt} → {1, . . . ,M} stipulating the data associations among St and L. Unlike in the geometric case, we cannot simply track the semantic features because we are interested in recovering their global positions L, which requires loop closure (recognizing previously observed landmarks instead of instantiating new ones). We focus on mapping semantic landmarks because they are fewer (leading to efficient performance) and yet more robust to viewpoint changes than geometric ones. This allows us to perform accurate visual-inertial odometry relying on both geometric and semantic features locally and efficient global loop closure over the map of semantic landmarks (Fig. 1).

To obtain the poses of the objects associated with the semantic landmarks, we use a representation similar to the deformable parts model [Felzenszwalb et al., 2010]. We introduce structure constraints among an object's landmarks, given by a root location and displacements of the object parts with respect to it (Fig. 3). For example, the front-left wheel of a car should not deviate significantly from an expected position relative to its front-left door. The structure constraint cij between the positions li, lj of two semantic parts belonging to the same object class are captured by a function h, while the part displacement costs are captured by a Gaussian model:

Given its semantic part positions, an object's orientation can be recovered via the Kabsch algorithm [Kabsch, 1978].

Problem. Determine the sensor trajectory ˆ X and semantic landmark positions ˆ L that maximize the likelihood of the inertial, geometric, and semantic measurements Z := {(It, Yt, St)}Tt=0 under the nonlinear Gaussian models (1)-(4)

2.2 Inference

A factor graph is a probabilistic graphical model [Koller and Friedman, 2009] that allows representing an optimization problem such as (5) as a graph of random variables (nodes) with constraints among them (factors). In our case, the nodes are Gaussian variables corresponding to X and L. The factors are constraints imposed by the inertial It, geometric Yt, and semantic St measurements via the models (1)-(4). Fig. 4 shows an example graph of two sensor states related by inertial and semantic measurements. Inference over the graph can be formulated as a maximization (5) of the likelihood of all measurements with respect to the latent variables X , L:

where we have skipped prior and structure constraints (4) to save and have used ‖e‖2Σ := eT Σ-1 e. The second equality utilizes that the models (1)-(4) are Gaussian; e.g., p(it+1 | xt+1, xt) ∝ exp {- 12 ‖it+1 - f (xt+1, xt)‖2U }. We also emphasized the dependence on the unknown data associations πt, dt via the indicator functions. Assuming for now that πt, dt are known, (6) is a nonlinear least-squares problem, which can be solved efficiently via incremental linearization (e.g., Gauss-Newton algorithm). Given initial estimates X 0 , L0 , e.g., obtained from IMU integration and feature triangulation, the functions f and g are linearized to obtain the Jacobians F x , F y and Gx , Gy with respect to each input, leading to:

To avoid estimating the geometric landmark positions {λj}, a key idea used in structureless VIO approaches [Mourikis and Roumeliotis, 2006] is to multiply the second term above by a unitary matrix whose columns form the the basis of the left nullspace of Gyt,j , making the term Gyt,j δλj disappear. Collecting the Jacobians into one large but sparse matrix A and the error vectors it -f (x0t , x0t-1 ), ytk -g(x0t , λ0j ), and stn - g(x0t , l0m) into one vector b, leads to a standard least-squares problem minθ ‖Aθ -b‖2 where θ := (δX , δL). Even though we are estimating the whole history X of sensor states, since the models (1)-(4) depend only on pairs of sensor-landmark states, A remains sparse and the problem can be solved efficiently via QR factorization. The optimization can be performed incrementally as new measurements arrive by updating (rather than recomputing) the QR factorization [Kaess et al., 2008; 2012].

2.3 Data Association

We return to the data association problem. Since we do not aim to recover {λj}, it is sufficient to obtain the geometric associations πt via feature tracking as mentioned in Sec. 2.1. It is, however, necessary to estimate the semantic associations D := {dt}Tt=0 , if we are to perform loop closure and reconstruct the semantic landmark positions L. We revisit the max likelihood estimation in (5), emphasizing that the semantic associations D are unknown and should also be inferred:

Existing work decomposes this joint SLAM and data association problem into two subproblems. First, given prior estimates X 0 ,L0 , the max likelihood estimate ˆD of the associations is computed, e.g., via JCBB [Neira and Tard´os, 2001] or the Hungarian algorithm [Munkres, 1957]. Then, given ˆD, the most likely landmark and sensor states are estimated. This is a special case of coordinate descent:

which allows revisiting association decisions once state estimates improve but does little to resolve the problem with ambiguous measurements since a hard decision on data associations is required and may have a highly detrimental effect on the estimation performance. Rather than simply selecting D as the mode of p(D | X,L,Z), our second innovation is to consider the entire distribution of D when estimating X and L. Since D is a latent variable whose realization we are not interested in inferring but is needed to estimate X and L, probability theory suggests that we should marginalize D. Our main result is that expectation maximization (EM) allows us to maximize the expected (w.r.t. D) measurement likelihood, utilizing the whole distribution of D:

To compare this with coordinate descent (8), we drop the inertial and geometric terms in (6) since their data associations are known or estimated via feature tracking and re-write (9):

Above Dt(n,m) := {dt | dt(n) = m} is the set of possible data associations at time t that assign semantic feature stn to landmark lm and wt,i n,m quantifies the probability that stn was produced by landmark lm via (3). Note that (6) and (8) have a similar form to (10), except that for each n there is exactly one m such that wt,i n,m = 1 and wt,i n,m′ = 0 for all m′ ̸= m. The EM formulation has the advantage that no hard decisions are required since it "averages" over all possible data associations. The following proposition shows that, besides generalizing coordinate descent, EM can be performed efficiently via a connection between the data association distribution wt,i n,m and the permanent 1 of a suitable matrix.

Proposition. Optimization (10) can be performed by iterating:

· E step: given X i, Li compute the associations distribution:

where Qt is a matrix of data association probabilities 2 with elements Qtnm and Qt-nm is the matrix with the nth row and mth column removed.

• M step: given the data association distribution ωt,i n,m, update the sensor and landmark state estimates:

This result allows us to take advantage of matrix permanent approximation algorithms [Jerrum et al., 2004] to efficiently summarize the combinatorial data association space in polynomial time. Instead of max likelihood data association as in (8), we estimate a data association distribution via the weights wt,i nm ("E" step) and then maximize the expected measurement log likelihood over it ("M" step).

1 The permanent of an n×m matrix A = [A(i, j)] with n ≤ m is defined as per(A) := ∑d ∏ni=1 A(i, d(i)), where the sum is over all one-to-one functions d : {1, . . . , n} → {1, . . . ,m}.

2Qt := [Gt Ht ], where Gt ∈ RNt×M is a matrix of scaled likelihoods Gtnm = ptm λ(1-ptn) p(stn | xt, lm), ptm is the probability that lm is detected from xt according to a Poisson process with mean λ, and Ht = diag ([pnew(st1) · · · pnew(stNt )]) ∈ RNt×Nt is a matrix of probabilities that new landmarks should be created. See [Atanasov et al., 2015] for details.

3 Evaluation

Our algorithm uses GTSAM and its iSAM2 implementation [Kaess et al., 2012] to perform the M step. Our frontend selects every 15th camera frame as a keyframe and tracks geometric ORB features. Outlier tracks that do not fit the essential matrix constraint between consecutive views are eliminated via RANSAC. The essential matrix is estimated using relative orientation from gyro measurements and two-point correspondences. Objects are detected via the deformable parts model [Dubout and Fleuret, 2013] and semantic features are extracted via a stacked hourglass convolutional network [Pavlakos et al., 2017]. Experiments were performed indoors, building a map of doors, swivel chairs and fourlegged chairs, and outdoors, building a map of cars on the KITTI odometry dataset [Geiger et al., 2012]. The indoor experiments included loops around a room equipped with motion tracking, a medium trajectory (175 m) spanning one floor, and a long trajectory (625 m) spanning two floors. The estimated sensor trajectory and chair poses in the medium sequence are shown in Fig. 5. The performance is compared qualitatively to ROVIO [Bloesch et al., 2015] and ORBSLAM2 [Mur-Artal and Tard´os, 2017]. Since ORB-SLAM2 uses only geometric features and no inertial information, it loses tracking for long durations in feature-deprived areas but is able to recover when entering previously mapped regions. In the long indoor sequence due to the repetitive nature of the hallways, the bag-of-words loop-closure approach of ORB-SLAM2 also makes incorrect associations leading to unsuccessful tracking. The outdoor experiments used visual odometry from VISO2 [Geiger et al., 2011] instead of inertial odometry. The translation and rotation errors of our algorithm, VISO2, and ORB-SLAM2 with respect to the ground truth vehicle trajectory are compared in Fig. 6 and Fig. 7.