This paper reports on the state of the art in underground SLAM by discussing different SLAM strategies and results across six teams that participated in the three-year-long SubT competition. In particular, the paper has four main goals. First, we review the algorithms, architectures, and systems adopted by the teams; particular emphasis is put on lidar-centric SLAM solutions (the go-to approach for virtually all teams in the competition), heterogeneous multi-robot operation (including both aerial and ground robots), and real-world underground operation (from the presence of obscurants to the need to handle tight computational constraints). We do not shy away from discussing the dirty details behind the different SubT SLAM systems, which are often omitted from technical papers. Second, we discuss the maturity of the field by highlighting what is possible with the current SLAM systems and what we believe is within reach with some good systems engineering. Third, we outline what we believe are fundamental open problems, that are likely to require further research to break through. Finally, we provide a list of open-source SLAM implementations and datasets that have been produced during the SubT challenge and related efforts, and constitute a useful resource for researchers and practitioners.
* 21 pages including references. This survey paper is submitted to IEEE
Transactions on Robotics for pre-approval
In this paper, we propose a Poisson multi-Bernoulli (PMB) filter for extended object tracking (EOT), which directly estimates the set of object trajectories, using belief propagation (BP). The proposed filter propagates a PMB density on the posterior of sets of trajectories through the filtering recursions over time, where the PMB mixture (PMBM) posterior after the update step is approximated as a PMB. The efficient PMB approximation relies on several important theoretical contributions. First, we present a PMBM conjugate prior on the posterior of sets of trajectories for a generalized measurement model, in which each object generates an independent set of measurements. The PMBM density is a conjugate prior in the sense that both the prediction and the update steps preserve the PMBM form of the density. Second, we present a factor graph representation of the joint posterior of the PMBM set of trajectories and association variables for the Poisson spatial measurement model. Importantly, leveraging the PMBM conjugacy and the factor graph formulation enables an elegant treatment on undetected objects via a Poisson point process and efficient inference on sets of trajectories using BP, where the approximate marginal densities in the PMB approximation can be obtained without enumeration of different data association hypotheses. To achieve this, we present a particle-based implementation of the proposed filter, where smoothed trajectory estimates, if desired, can be obtained via single-object particle smoothing methods, and its performance for EOT with ellipsoidal shapes is evaluated in a simulation study.
This paper presents a general solution for computing the multi-object posterior for sets of trajectories from a sequence of multi-object (unlabelled) filtering densities and a multi-object dynamic model. Importantly, the proposed solution opens an avenue of trajectory estimation possibilities for multi-object filters that do not explicitly estimate trajectories. In this paper, we first derive a general multi-trajectory backward smoothing equation based on random finite sets of trajectories. Then we show how to sample sets of trajectories using backward simulation for Poisson multi-Bernoulli filtering densities, and develop a tractable implementation based on ranked assignment. The performance of the resulting multi-trajectory particle smoothers is evaluated in a simulation study, and the results demonstrate that they have superior performance in comparison to several state-of-the-art multi-object filters and smoothers.
* Accepted for publication in IEEE Transactions on Signal Processing
We propose a scalable track-before-detect (TBD) tracking method based on a Poisson/multi-Bernoulli model. To limit computational complexity, we approximate the exact multi-Bernoulli mixture posterior probability density function (pdf) by a multi-Bernoulli pdf. Data association based on the sum-product algorithm and recycling of Bernoulli components enable the detection and tracking of low-observable objects with limited computational resources. Our simulation results demonstrate a significantly improved tracking performance compared to a state-of-the-art TBD method.
This paper presents a factor graph formulation and particle-based sum-product algorithm (SPA) for scalable detection and tracking of extended objects. The proposed method efficiently performs probabilistic multiple-measurement to object association, represents object extents by random matrices, and introduces the states of newly detected objects dynamically. Scalable detection and tracking of objects is enabled by modeling association uncertainty by measurement-oriented association variables and newly detected objects by a Poisson birth process. Contrary to conventional extended object tracking (EOT) methods with random-matrix models, a fully particle-based approach makes it possible to represent the object extent by different geometric shapes. The proposed method can reliably determine the existence and track a large number of closely-spaced extended objects without gating and clustering of measurements. We demonstrate significant performance advantages of our method compared to the recently proposed Poisson multi-Bernoulli mixture filter in a challenging tracking scenario with ten closely-spaced extended objects.
This paper proposes a Poisson multi-Bernoulli mixture (PMBM) filter for coexisting point and extended targets. The PMBM filter provides a recursion to compute the filtering posterior based on single-target predictions and updates. In this paper, we first derive the PMBM filter update for a generalised measurement model, which can include measurements originated from point and extended targets. Second, we propose a single-target space that accommodates both point and extended targets and derive the filtering recursion that propagates Gaussian densities for single targets and gamma Gaussian inverse Wishart densities for extended targets. As a computationally efficient approximation of the PMBM filter, we also develop a Poisson multi-Bernoulli (PMB) filter for coexisting point and extended targets. The resulting filters are analysed via numerical simulations.
This paper presents two trajectory Poisson multi-Bernoulli (TPMB) filters for multi-target tracking: one to estimate the set of alive trajectories at each time step and another to estimate the set of all trajectories, which includes alive and dead trajectories, at each time step. The filters are based on propagating a Poisson multi-Bernoulli (PMB) density on the corresponding set of trajectories through the filtering recursion. After the update step, the posterior is a PMB mixture (PMBM) so, in order to obtain a PMB density, a Kullback-Leibler divergence minimisation on an augmented space is performed. The developed filters are computationally lighter alternatives to the trajectory PMBM filters, which provide the closed-form recursion for sets of trajectories with Poisson birth model, and are shown to outperform previous multi-target tracking algorithms.
This paper presents the Gaussian implementation of the multi-Bernoulli mixture (MBM) filter. The MBM filter provides the filtering (multi-target) density for the standard dynamic and radar measurement models when the birth model is multi-Bernoulli or multi-Bernoulli mixture. Under linear/Gaussian models, the single target densities of the MBM mixture admit Gaussian closed-form expressions. Murty's algorithm is used to select the global hypotheses with highest weights. The MBM filter is compared with other algorithms in the literature via numerical simulations.
We provide a derivation of the Poisson multi-Bernoulli mixture (PMBM) filter for multi-target tracking with the standard point target measurements without using probability generating functionals or functional derivatives. We also establish the connection with the \delta-generalised labelled multi-Bernoulli (\delta-GLMB) filter, showing that a \delta-GLMB density represents a multi-Bernoulli mixture with labelled targets so it can be seen as a special case of PMBM. In addition, we propose an implementation for linear/Gaussian dynamic and measurement models and how to efficiently obtain typical estimators in the literature from the PMBM. The PMBM filter is shown to outperform other filters in the literature in a challenging scenario.
* IEEE Transactions on Aerospace and Electronic Systems, vol. 54,
no. 4, pp. 1883-1901, Aug. 2018
Data association, the reasoning over correspondence between targets and measurements, is a problem of fundamental importance in target tracking. Recently, belief propagation (BP) has emerged as a promising method for estimating the marginal probabilities of measurement to target association, providing fast, accurate estimates. The excellent performance of BP in the particular formulation used may be attributed to the convexity of the underlying free energy which it implicitly optimises. This paper studies multiple scan data association problems, i.e., problems that reason over correspondence between targets and several sets of measurements, which may correspond to different sensors or different time steps. We find that the multiple scan extension of the single scan BP formulation is non-convex and demonstrate the undesirable behaviour that can result. A convex free energy is constructed using the recently proposed fractional free energy (FFE). A convergent, BP-like algorithm is provided for the single scan FFE, and employed in optimising the multiple scan free energy using primal-dual coordinate ascent. Finally, based on a variational interpretation of joint probabilistic data association (JPDA), we develop a sequential variant of the algorithm that is similar to JPDA, but retains consistency constraints from prior scans. The performance of the proposed methods is demonstrated on a bearings only target localisation problem.