On Reference Solutions to Non-Gaussian SLAM Factor Graphs

by   Qiangqiang Huang, et al.

Many real-world applications of simultaneous localization and mapping (SLAM) require approximate inference approaches, as exact inference for high-dimensional non-Gaussian posterior distributions is often computationally intractable. There are substantial challenges, however, in evaluating the quality of a solution provided by such inference techniques. One approach to solution evaluation is to solve the non-Gaussian posteriors with a more computationally expensive but generally accurate approach to create a reference solution for side-by-side comparison. Our work takes this direction. This paper presents nested sampling for factor graphs (NSFG), a nested-sampling-based approach for posterior estimation in non-Gaussian factor graph inference. Although NSFG applies to any problem modeled as inference over a factor graph, we focus on providing reference solutions for evaluation of approximate inference approaches to SLAM problems. The sparsity structure of SLAM factor graphs is exploited for improved computational performance without sacrificing solution quality. We compare NSFG to two other sampling-based approaches, the No-U-Turn sampler (NUTS) and sequential Monte Carlo (SMC), as well as GTSAM, a state-of-the-art Gaussian SLAM solver. We evaluate across several synthetic examples of interest to the non-Gaussian SLAM community, including multi-robot range-only SLAM and range-only SLAM with ambiguous data associations. Quantitative and qualitative analyses show NSFG is capable of producing high-fidelity solutions to a wide range of non-Gaussian SLAM problems, with notably superior solutions than NUTS and SMC. In addition, NSFG demonstrated improved scalability over NUTS and SMC.



There are no comments yet.


page 1


Online Incremental Non-Gaussian Inference for SLAM Using Normalizing Flows

This paper presents a novel non-Gaussian inference algorithm, Normalizin...

NF-iSAM: Incremental Smoothing and Mapping via Normalizing Flows

This paper presents a novel non-Gaussian inference algorithm, Normalizin...

WOLF: A modular estimation framework for robotics based on factor graphs

This paper introduces WOLF, a C++ estimation framework based on factor g...

Linear SLAM: Linearising the SLAM Problems using Submap Joining

The main contribution of this paper is a new submap joining based approa...

Statistical Outlier Identification in Multi-robot Visual SLAM using Expectation Maximization

This paper introduces a novel and distributed method for detecting inter...

A Spectral Learning Approach to Range-Only SLAM

We present a novel spectral learning algorithm for simultaneous localiza...

Fast Uncertainty Quantification for Active Graph SLAM

Quantifying uncertainty is a key stage in autonomous robotic exploration...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Precise state-estimation and uncertainty quantification are necessary capabilities for robotic navigation. However, these are nontrivial tasks, as the posterior distribution of the robot’s state is generally non-Gaussian due to nonlinearities in the sensing models. In cases where the posterior distribution heavily concentrates at a single point in the state-space (i.e. the first two moments of the distribution capture most of the behavior) it can be reasonable to treat the distribution as if it were a unimodal Gaussian. Many algorithms exploit this approach, performing

maximum a posteriori (MAP) estimation and approximating the uncertainty as Gaussian via a Laplace approximation [kaess09ras, kummerle2011g, kaess12ijrr, dellaert17factor]. However, Laplace approximations possess limited expressiveness, and cannot represent the non-Gaussian posteriors seen in many real-world problems such as range-only SLAM and multi-modal data association.

Recognition of this issue has driven the non-Gaussian SLAM community to develop inference techniques which flexibly represent posterior distributions of the robot and landmark states [blanco10ijrr, fourie20iros, huang21icra]

. However, given the relatively large dimensionality of SLAM problems of interest and the complexity of the posterior distribution, it is challenging to evaluate the solutions provided by such inference techniques. Many non-Gaussian inference techniques use sampling based methods to represent the joint-posterior with a number of samples. Unfortunately, checking whether a set of samples well represents a general probability distribution – commonly referred to as goodness of fit (GOF) testing – is still an open question. Recent works in GOF testing to compare samples to a known distribution have developed strong theoretical foundations

[liu16icml, gorham19appliedprobability, anastasiou2021stein], but current approaches lack practical scalability. Specifically, the discriminative power of these tests severely diminishes in the low-sample, high-dimensional problems of interest to the SLAM community [anastasiou2021stein].

Fig. 1: Our approach to performing nested sampling over factor graphs. (a) The factor graph is first decomposed into two parts: an acyclic graph, and a set of loop-closing factors. Including any of the loop-closing factors would cause the acyclic graph to become cyclic. (b) From the acyclic graph we apply ancestral sampling to generate samples of the prior distribution. A likelihood function is constructed from the loop-closing factors. The functions for sampling and likelihood evaluation are supplied to a nested sampler. Nested sampling returns weighted samples, which approximate the joint posterior distribution.

An alternative evaluation technique compares the inferred solution to a high-quality reference solution, a set of samples which generally well represent the underlying distribution [gretton2012kernel]. Such reference solutions enable qualitative and quantitative evaluation of the inferred solution. Although producing such reference solutions is effectively solving a non-Gaussian SLAM problem, there are notable practical differences. Namely, as the key concern is offline evaluation of solution quality, the need for computational efficiency is greatly reduced. As a result, generating reference solutions can consider a much larger class of algorithms than otherwise available to general runtime-constrained SLAM systems.

We propose a novel technique for generating such reference solutions. In this work we present nested sampling for factor graphs (NSFG), an approach which combines structural insights from factor graphs with off-the-shelf nested sampling solutions to perform inference over factor graphs. We evaluate NSFG’s potential to produce reference solutions for non-Gaussian SLAM problems. We compare NSFG to two other general inference algorithms based on the No-U-Turn Sampler and sequential Monte Carlo as well as the state-of-the-art Gaussian SLAM solver GTSAM [dellaert2012factor]. We partially mitigate the substantial computational complexity of the sampling-based approaches by leveraging the conditional independence structure revealed by the factor graph representation. We evaluate the ability of these algorithms to produce reference solutions on the following set of SLAM problems: pose-graph, range-only, multi-robot range-only, and range-only with ambiguous data-associations. We make the implementations and data for all of our experiments available.222

Ii Related Work

Many techniques including non-linear least squares (NLLS) have been applied to obtain Laplace approximations of the posteriors in the SLAM problem (i.e. Gaussian SLAM). State-of-the-art methods commonly leverage factor graphs to represent the conditional independence structure of the problem [dellaert17factor]. These factor graphs intuitively describe the sparsity pattern induced by conditional independence in the problem [koller09probabilistic]. This has been used to develop efficient Gaussian SLAM algorithms which effectively solve the SLAM problem as a set of smaller, loosely-connected sub-problems by incremental updates [kaess12ijrr]. While these solvers possess ideal computational efficiencies, they are limited to unimodal Gaussian representations of the posterior distribution and thus cannot represent a large class of non-Gaussian SLAM problems. Moreover, these solvers may even fail to return a Laplace approximation of posteriors if the information matrix for NLLS is under-determined or reasonable initial values for the optimization cannot be obtained (e.g. range-only SLAM).

Although Laplace approximations prevail in many SLAM applications, the development of a general non-Gaussian framework for SLAM state estimation has long been recognized as a key objective in the community. Early work in SLAM state estimation focused on variants of Kalman filtering

[gordon1993novel, durrant2003bayesian, eustice05icra] and particle filtering [blanco10ijrr, montemerlo2003fastslam]

. More recent work has leveraged factor graph representations and conditional independence to perform efficient smoothing-type inference over the full time-history of the robot measurements via kernel density estimation 

[fourie20iros], multi-hypothesis tracking [hsiao19icra], belief propagation [Ortiz2021visualGBP], and normalizing flows [huang21icra]. Beyond general SLAM algorithms, many works focus on specific instances of non-Gaussian SLAM, particularly range-only SLAM [djugash06icra, blanco08icra] and ambiguous data-associations [olson13ijrr, neira2001tra, sunderhauf12iros, bowman17icra, doherty19icra], where the algorithms can be tuned to the unique type of non-Gaussianity that arises in each instance.

However, despite the range of non-Gaussian SLAM solvers available, as mentioned in the introduction, evaluating the accuracy of general non-Gaussian inference solutions is still an active area of research [liu16icml, gorham15neurips, gorham2017icml, gong21arxiv] and not currently well-equipped for the high-dimensional problems encountered in SLAM [anastasiou2021stein]. This draws our interests to more general sampling-based inference techniques for computing reference solutions, from which we can evaluate the quality of approximate solutions via maximum mean discrepancy (MMD) [gretton2012kernel].

Due to the cyclic graphical structure and non-Gaussian posteriors in SLAM problems, conventional sampling-based inference methods such as inverse transform sampling and ancestral sampling [bishop2006pattern]

generally cannot be applied. Hence, we need techniques such as nested sampling or Markov chain Monte Carlo (MCMC), which can sample the posterior distribution despite the cyclic structure. MCMC is a broad family of sampling algorithms using a Markov process to explore the state space and generate samples subject to target densities

[mackay98introduction]. Its classic version, Metropolis-Hastings MCMC, has been applied to SLAM problems [torma10aistats, shariff15aistats]. Hamiltonian Monte Carlo (HMC) is another MCMC variant exploiting the gradient of the target density to guide the sampling exploration [betancourt18conceptual]. However, fine tuning of HMC parameters is necessary for achieving a compelling sampling efficiency. The No-U-Turn Sampler (NUTS) [hoffman2014jmlr] extends HMC with automatic parameter tuning, improving ease of use and sampling efficiency. However, multi-modal distributions with distant modes still pose difficulties to the algorithm since only local density information guides the state space exploration.

Sequential Monte Carlo (SMC) is a sampling algorithm which combines importance sampling, re-sampling, tempering, and MCMC. Particle filters in general are just special instances of SMC [doucet2009tutorial]

. SMC usually requires a good proposal distribution that possesses good coverage of the typical set of the target density. However, for a high dimensional density, there is low probability that a number of samples of the proposal distribution cover the typical set.

Nested sampling was proposed by Skilling [skilling2006nested]

to compute the evidence or marginal likelihood for Bayesian inference with a by-product of posterior samples. It is mostly developed and used in the field of astronomy. Two of its features are very attractive: (i) well-defined stopping criteria related to the convergence of estimated evidence, and (ii) a global exploration of the state space. This global exploration has found great success with complex posterior distributions such as those in non-Gaussian SLAM problems. There are several open-source nested sampling packages

[feroz2009multinest, handley2015polychord, speagle2020dynesty]. In particular, Speagle recently developed dynesty [speagle2020dynesty], a well-documented and algorithmically advanced dynamic nested sampling package. Our approach for non-Gaussian SLAM exploits the nested sampling implementation provided by dynesty.

Iii Methods

Iii-a Factor graphs and the problem formulation

The latent variable,

, denotes a high-dimensional random variable consisting of all robot pose variables (

) and landmark location variables (). Our goal is to estimate the posterior distribution of after considering all measurements, i.e. . Typical SLAM problems can elegantly work with this posterior density through factor graphs [dellaert17factor], graphical models comprised of factors and variables. We present an example factor graph SLAM representation in LABEL:fig:_factorgrapha.

A factor represents a likelihood function over one or more variables. In general we write to indicate the one or more variables related to factor . We write the likelihood functions as in the case of measurements and as in the case of prior distributions not related to measurements. The joint posterior then relates to our factors as follows, with total factors in the graph:


For intuition, in Fig. 1a, with robot poses and landmark positions , the factors represent landmark observations, the factors represent odometry measurements, and the factor represents the prior on the robot’s starting position.

Iii-B Nested sampling

The basic idea of nested sampling is integration of the likelihood function, , over the volume, , of the prior distribution, , as follows:


Note that here can be different from the nominal likelihood and, accordingly, can incorporate more factors than the nominal prior . We take advantage of this in our approach by constructing a simple but informative prior to reduce the complexity of the likelihood

. The rightmost integration can be interpreted as partitioning the entire prior probability into small prior volumes,

, and ranking them by likelihood. As the contribution of our work is not in performing nested sampling, but in using conditional independence structures to more efficiently prepare a problem for nested sampling we refer to [skilling2006nested, speagle2020dynesty, buchner2021nested] for more details about nested sampling methods.

Iii-C Structured factors for sampling

While nested sampling provides a powerful approach to estimate the posteriors for complex non-Gaussian inference problems, the application of the technique to the factor graphs encountered in SLAM is not straightforward. We focus on how to structure the SLAM problem to enable nested sampling to be effectively applied to the problem. Specifically, we exploit the sparsity structure of SLAM factor graphs to construct a and which are more computationally efficient for nested sampling.

A cyclic factor graph can be viewed as an acyclic factor graph accompanied by loop-closing edges (see Fig. 1b). Correspondingly, we define the factors in the acyclic factor graph that incorporates all variables as the ‘AC’ set while the remaining factors are grouped as ‘LC’ set. From the perspective of density factorization, the posterior density in (1) can be equivalently written as


Furthermore, we construct the prior density in nested sampling following


and leave the loop-closing factors to the likelihood function


This partition is helpful for nested sampling due to two aspects: (i) the prior density resembles the posterior distribution better than simple proposals such as uniform distributions while ancestral sampling can be applied to draw prior samples along the acyclic structure with tractable computational cost, and (ii) the likelihood function involves fewer factors, which reduces the cost of likelihood evaluation in nested sampling. SLAM factor graphs are usually sparse which implies that the cardinality of AC set can be comparable to or even much greater than the cardinality of LC set. Therefore, exploiting the sparsity structure of SLAM factor graphs can effectively improve the computational performance of nested sampling without sacrificing solution quality.

Iv Implementation

Iv-a Observation models

Although all the factors in our experiments assume Gaussian additive noise in the measurement models, they each result in non-Gaussian distributions. For objects such as robot poses or relative transformations, which involve the Lie groups

or , we assume that the Gaussian noise exists in the tangent space (i.e. Lie algebra) of the group as is standard [boumal20book]. For example, a noisy pose observation is defined as follows, with indicating the exponential map operator:


where is a latent variable, is a noisy observation, and is the perturbation subject to a Gaussian distribution. Although the noise is Gaussian, the exponential map in (6) introduces non-linearity, leading to a non-Gaussian posterior. Similarly, the nonlinearity in the operator of the distance measurement model described in (7) introduces a non-Gaussian posterior in range measurements despite Gaussian additive noise.


where is the translation component of variable or and is a noisy distance measurement. Beyond pose and distance observations we will also make use of factors to represent ambiguous data associations. To do so, we represent each possible association with equally weighted mixture models, which can be written as


where denotes possible data associations.

Iv-B Inverse transforms and ancestral sampling

As all of our noise models are Gaussian, we have closed-form quantile functions to transform uniformly-distributed variables to the perturbation variables for our observations. This is critical, as nested sampling begins by uniformly drawing samples from a hypercube which then need be mapped to the noisy perturbations in the observation models. With these mappings and our observation models from (

6, 7) we can propagate the samples along the acyclic factor graph via ancestral sampling [buchner2021nested]. Because we can easily propagate samples in this manner, and the ancestral sampling informatively generates these samples, this allows us to efficiently construct an informative prior through these samples. This informative prior is one of the major strengths of our approach.

Iv-C NSFG and baseline solvers

We implemented Algorithms 1 and 2 for obtaining structured factors and drawing posterior samples via NSFG. Note that we use a package, dynesty [speagle2020dynesty], for performing nested sampling in NSFG. We supply the gradient of our factors to the NUTS and SMC implementations in PyMC3 [salvatier16probabilistic] to solve our SLAM problems. We used the default built-in initialization functions in PyMC3 for NUTS and supplied a predetermined uniform distribution that covers the space of interests to SMC. GTSAM was always initialized with the groundtruth pose and landmark values. All sampling-based techniques were implemented in Python while the C++ library of GTSAM [dellaert2012factor] was used. All computation was run on an AMD Ryzen ThreadRipper 3970X processor with 32 cores.

Input: Univariate factor set and all other factors
Output: Acyclic factor set and loop-closing set
Initialize empty FIFO queues of acyclic-graph factors , loop-closing factors , and acyclic-graph variables   // Push prior factors to AC
1 while  do
         // Pop the first element
2       if  connected with more than two variables then
               // Push multi-modal factor to
4       else if all variables connected to in  then
               // Push loop-closing factor to
6       else if only one variable of in  then
8       else
return and
Algorithm 1 Decompose factor graphs
Input: Acyclic factor set and loop-closing set
Output: Samples of the joint posterior distribution
1 Function PriorTrans(hypercube sample ):
2      Initialize a dictionary for containing prior samples for  in  do
3             Variable in but not in
4      return
5 Function LLK(latent variable ):
         // Initialize log likelihood
6       for  in  do
8      return
return   // Return samples
Algorithm 2 NSFG

V Results

We compare NSFG to NUTS, SMC, and GTSAM on a range of synthetic 2D experiments with varying forms of non-Gaussian structure. To attempt to quantify the accuracy of the sampling-based approaches, we construct problems with approximately unimodal Gaussian posterior distributions. Specifically, we construct single-robot pose-graph SLAM problems with low rotational uncertainty and single-robot range-only SLAM problems, where we only quantitatively evaluate the accuracy of the range-only solutions at the final time step after the posterior has collapsed to a unimodal distribution. In these problems we compare the sample-based solutions to samples drawn from the Laplace approximation provided by GTSAM. In our evaluation we compute the root mean squared error (RMSE) of the sampled means to the MAP estimate from GTSAM as well as the maximum mean discrepancy (MMD) between the sampled distributions and the samples drawn from the Laplace approximation.

In addition, we evaluate NSFG’s ability to provide solutions for highly complex distributions. We use NSFG to solve multi-robot SLAM problems with inter-robot ranging, and single-robot range-only SLAM problems with multiple beacons and ambiguous data-association on the range measurements.

For the single-robot pose-graph SLAM problems, as seen in Fig. 2, all solutions were qualitatively similar with exception to some spurious modes found in the NUTS solution. Quantitative analysis, shown in Fig. 3, suggested that, in terms of both MMD and RMSE, NSFG was best able to match the solution provided by GTSAM. Although runtime is not an explicit concern, we note that NSFG was consistently much faster than NUTS or SMC, suggesting it is capable of scaling to larger problems than NUTS or SMC.

We show a single result from the single-robot range-only SLAM problems in Fig. 4. In the first three time steps (X0, X1, X2) the robot is moving along a line, as such the posterior distribution of the landmark position consists of two distinct modes mirrored across the line being driven by the robot. At the final time step (X3) the robot breaks away from the line, disambiguating the landmark position and causing the posterior distribution to statistically converge to a single mode around the true landmark position. Qualitative evaluation of the solutions showed that NSFG was the only technique which appeared to match the expected posterior distribution for all time steps observed. GTSAM is only capable of providing a unimodal Gaussian distribution, as seen in the figure.

Fig. 2: Samples for estimated posterior distributions for a pose-graph with 6 poses and 4 loop-closures.
Fig. 3: Performance of different sampling-based approaches: (a) statistics are from 10 randomly generated pose graphs with 30 dimensions each (10 poses), and (b) statistics are from 10 randomly generated single-robot range-only SLAM problems with 14 dimensions (4 poses and 1 landmark position). The shaded area indicates the confidence interval.

NUTS appeared to underweight the equally likely mode to the right of the robot and incorrectly eliminated the second mode in time step 2 before the landmark had been disambiguated. SMC had contrasting issues, as it appeared to capture both modes accurately for time step 2 but failed to eliminate the incorrect mode after the landmark had been disambiguated in time step 3.

In Fig. 3 we quantitatively compare NSFG, NUTS, and SMC on 10 other randomly generated single-robot range-only experiments. The statistics presented were computed at the final time step of each experiment after the distribution would be expected to be unimodal (similar to time step 3 in Fig. 4). In general, NSFG outperformed SMC and NUTS, displaying the lowest median values and range across all three metrics. In Fig. 5 we display diagnostics explaining the issues observed in SMC and NUTS in Fig. 4. As observed in the two plots on the right of Fig. 5, the MCMC chains are effectively stuck in local optima. These local optima prevent the samplers from exploring the state space and prevent the MCMC chains from mixing between the two modes of the distribution. In the case of the NUTS solution at time step 2 (Fig. 4), all of the MCMC chains are stuck in the single local optima, and are thus prevented from exploring and sampling the equally probable mirrored solution. In the case of the SMC solution at time step 3 (Fig. 4) although the incorrect landmark hypothesis has been eliminated, there is a local optima around the landmark position which the MCMC chains cannot escape. For this reason even though that hypothesis on landmark position has been effectively eliminated, these artifacts on the incorrect hypothesis remain.

Fig. 4: Solutions generated by each of the inference methods tested on a single-robot range-only SLAM problem with a single beacon. The red arrows indicate the groundtruth robot pose and the blue scatter points indicate samples of the estimated landmark state as provided by each of the respective solutions.
Fig. 5: Traces of the x-coordinate values of the landmark explored by the MCMC chains in Fig. 4 and kernel density estimation performed on the final distributions obtained from these MCMC chains. (a) NUTS solution for the baseline range-only problem at time step 2 and (b) SMC solution for the baseline range-only problem at time step 3.
Fig. 6: Scatter plots for posteriors of a three-robot range-only SLAM problem using our approach. Only the first four steps are shown. Red and black lines denote connections for range measurements and odometry respectively.

In Figs. 7 and 6 we evaluate NSFG solutions to a multi-robot SLAM problem with inter-robot ranging that contained 6 time steps. In each experiment range-measurements were provided between all robots at each time step, helping the solution converge to a well-concentrated unimodal distribution. As can be seen, the posterior distribution estimated by NSFG progresses from annular in time step 0, to multi-modal in time step 1, to increasingly unimodal, Gaussian in all later time steps. This is supported by the RMSE plot in Fig. 7, in which the RMSE drops substantially for all steps after time step 1 because the posterior distribution has collapsed to a unimodal state.

Fig. 7: Runtime and RMSE plots across 10 multi-robot range-only SLAM problems using NSFG. There are 18 poses (54 dimensions) at the final time step. The shaded area indicates the confidence interval.

In Figs. 9 and 8 we evaluate the performance of NSFG on a single-robot SLAM problem with range-measurements to one of two beacons after time step 0, however from pose to the landmark association is ambiguous (i.e. the robot is unsure of which landmark the distance measurement goes to). In ground truth and observe while and observe . NSFG displays the complex posterior distribution that arises from this situation and is capable of disambiguating the measurement associations by time step 7, as seen in Fig. 8. This posterior considering data-association ambiguity can be formulated by a mixture model as shown in


where denotes the set of all combinations of data association. Every component in the mixture essentially stems from a posterior distribution under one of the combinations, i.e., . We can utilize this fact to check the consistency of NSFG’s performance.

Fig. 8: Scatter plots for posteriors of a range-only SLAM problem with data association ambiguity using our approach. Dashed red lines denote uncertain data association of range measurements.
Fig. 9: Weights and posteriors under different data associations. (a) and (b) are estimated weights of data association hypotheses at time step 2 and 4 respectively. (c) and (d) are posterior samples formed by re-sampling over weighted samples from individual components in the joint posterior.

Fixing the data association by a given combination, , results in a new factor graph without ambiguity. For the new factor graph, NSFG can draw samples representing its posterior, , and estimate its evidence, . As there is no prior on data associations, is assumed to be . Thus, the weights of components in (9), , can be computed if we apply NSFG to solve factor graphs resulted from all combinations of data association (Fig. 9, top). A new ensemble of samples representing the joint posterior can be formed by performing re-sampling over the samples and weights for different data associations, as seen in the bottom of Fig. 9. These scatter plots resemble their counterparts in Fig. 8 well. As such, we argue that these results support the claim that NSFG is self-consistent and returns accurate solutions even with complex posterior distributions.

Vi Conclusion and Future Work

We introduced nested sampling techniques to push the limit of exact inference for posterior estimation of non-Gaussian SLAM problems. Leveraging the sparsity structure of SLAM factor graphs, we refactored the posterior density to improve the efficiency of nested sampling. Our nested-sampling-based factor graph inference approach, NSFG, has shown accuracy and runtime advantages over state-of-the-art techniques.

We argue that NSFG can be a promising tool to provide reference solutions for non-Gaussian SLAM problems. Those reference solutions will be helpful for performance evaluation of approximate inference algorithms and deeper understanding of uncertainty propagation on cyclic non-Gaussian SLAM factor graphs. Equipped with NSFG, one could simulate and characterize uncertainty propagation beyond the acyclic pose graphs to which many existing works is limited [mangelson2020characterizing, kim2017uncertainty, barfoot2014associating, long2013banana].

Future work includes (i) implementation and experiments for 3D datasets, and (ii) tests on more non-Gaussian inference problems such as outlier rejection.