## 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].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.^{2}^{2}2

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

(1) |

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:

(2) |

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

(3) |

Furthermore, we construct the prior density in nested sampling following

(4) |

and leave the loop-closing factors to the likelihood function

(5) |

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:(6) |

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.

(7) |

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

(8) |

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.

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

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.

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.

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

(9) | ||||

(10) |

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.

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.

Comments

There are no comments yet.