Topological belief space planning for active SLAM with pairwise Gaussian potentials and performance guarantees

Abstract

Determining a globally optimal solution of belief space planning (BSP) in high-dimensional state spaces directly is computationally expensive, as it involves belief propagation and objective function evaluation for each candidate action. However, many problems of interest, such as active SLAM, exhibit structure that can be harnessed to expedite planning. Also, in order to choose an optimal action, an exact value of the objective function is not required as long as the actions can be sorted in the same way. In this paper we leverage these two key aspects and present the topological belief space planning (t-bsp) concept that uses topological signatures to perform this ranking for information-theoretic cost functions, considering only topologies of factor graphs that correspond to future posterior beliefs. In particular, we propose a highly efficient topological signature based on the von Neumann graph entropy that is a function of graph node degrees and supports an incremental update. We analyze it in the context of active pose SLAM and derive error bounds between the proposed topological signature and the original information-theoretic cost function. These bounds are then used to provide performance guarantees for t-bsp with respect to a given solver of the original information-theoretic BSP problem. Realistic and synthetic simulations demonstrate drastic speed-up of the proposed method with respect to the state-of-the-art methods while retaining the ability to select a near-optimal solution. A proof of concept of t-bsp is given in a small-scale real-world active SLAM experiment.

1. Introduction

Decision-making under uncertainty in partially observable domains is inescapable for autonomous systems operating in the real world. An autonomous agent (robot) needs to make predictions about its own state and state of the world given often partial and uncertain data and decide its own actions accordingly. In principle, it has to perform inference about unknown quantities and evaluate the task-related objective on available actions. These operations can be quite expensive, especially in high-dimensional state spaces and with information-theoretic cost functions, such as differential entropy. Online decision-making under uncertainty in such a setting is essential in numerous problems in robotics, such as single and multi-robot active SLAM, sensor deployment, search and rescue, and autonomous environmental monitoring.

However, many problems of interest, for example, active SLAM, exhibit structure. In such problems, only subsets of variables interact directly with each other. It means there exist conditional independencies between variables and it is possible to simplify the model of their joint probability distribution. One way to represent certain dependencies or causality among variables of a stochastic system is via a graphical model of their joint probability distribution (Koller and Friedman 2009), for example, a factor graph and a Markov Random Field.

A graphical model approach involves expressing the joint probability density/mass function (belief) as a product of local functions of its variables. A structure of this model, described by a topology induced from the connectivity of the variables and factors they belong to, reveals probabilistic and algorithmic features of the model (e.g., independence, learning complexity) often indicating how to efficiently perform marginalization and conditioning operations on that model using only local computations (Lauritzen and Spiegelhalter 1988).

Graphical models have been much investigated in a design of efficient inference and learning algorithms (Sucar 2015), especially in the SLAM community (Folkesson and Christensen 2004; Frese 2006; Kaess et al. 2008, 2012; Thrun and Leonard 2008; Paskin 2003; Dellaert and Kaess 2017). However, a study of structural properties of probability distributions and their use in decision-making under uncertainty is relatively new (Kitanov and Indelman 2018; Chen et al. 2018, 2019; Khosoussi et al. 2019).

Moreover, sometimes it is possible to make decisions without explicitly performing the computationally expensive inference of future beliefs and cost function calculation by looking at some other features of the system highly related to the objective. Using this strategy, we can focus on important features first and leave the details for later if necessary, or when resources become available. The difficulty within this approach lies in identifying such features, that, on top of being discriminative, are less expensive to compute than solving the original optimization problem explicitly, and in understanding how close the solution based on them is to optimal.

In this work, we identify and study such features relevant to information-theoretic decision-making under uncertainty induced from structural information of the corresponding posterior probability distributions. Our approach is applicable to high-dimensional Gaussian belief space planning (BSP) problems that exhibit the specific structure of pairwise potentials, that is, each factor involves only two variables. It therefore particularly supports variants of the active SLAM problem (see, e.g., a recent survey (Placed et al. 2023)), that exhibit such a structure.

To get the intuition behind this idea, see Figure 1. Different actions generally lead to factor graphs with different topologies and more complex graphs tend to produce more accurate estimates, with smaller entropy. The idea is then to develop a computationally lightweight topological signature, that is, a function of the graph topology, that is highly correlated with the original information-theoretic cost, and to utilize that signature for decision-making. Such a concept is therefore expected to be especially beneficial in decision-making problems with large graphs, such as in active SLAM.

Figure 1.

Conceptual illustration of topological belief space planning. Given that the objective is uncertainty minimization of the trajectory, an optimal action can be often deduced solely from posterior’s topology (middle column), that is, without predicting the values of future measurements and performing belief propagation (right column), but only by predicting that the trajectory a₁ will have loop closures due to re-observing previously mapped area, while a₂ won’t as it leads to an unknown (gray shaded) area (left column).

1.1. Related work

BSP can be seen as a joint control and estimation problem in which an agent (robot) has to find an optimal control according to a specific task-related objective, which itself has to be estimated while accounting for different sources of uncertainty, for example, due to stochastic sensing, motion or environment. Some interesting instantiations of this problem are active SLAM (e.g., Atanasov et al., 2015; Du et al., 2011; Huang et al., 2005; Indelman et al., 2015; Kim and Eustice, 2014; Kopitkov and Indelman, 2017, 2019; Regev and Indelman, 2017; Stachniss et al., 2004; Valencia et al., 2012), active perception (Bajcsy 1988), sensor deployment and measurement selection (Joshi and Boyd 2009; Bian et al. 2006; Hovland and McCarragher 1997), graph reduction, and pruning (Kretzschmar and Stachniss 2012; Carlevaris-Bianco et al. 2014).

The general BSP problem with Markovian models can be naturally represented by a Partially Observable Markov Decision Process (POMDP). Determining globally optimal solutions to the POMDP problem is computationally intractable for most but the simplest real-world problems due to both the curse of dimensionality and the curse of history (see, e.g., Pineau et al. 2006). Computational complexity remains an issue even with simplifying assumptions on the probability distribution of the states, finite planning horizons, discrete states, actions or observations (Papadimitriou and Tsitsiklis 1987). As a result, a large subset of prior work has focused on approximately solving the POMDP problem to provide better scalability. Some examples include using sampling-based motion planners (e.g., Kavraki et al. 1996; Prentice and Roy 2009; Agha-Mohammadi et al. 2014; Davidson and Hutchinson 2009), local optimization methods for continuous state spaces (e.g., Indelman et al. 2015; Platt et al. 2010; Van Den Berg et al. 2012), or point-based value iteration (e.g., Pineau et al. 2006; Porta et al. 2006; Kurniawati et al. 2008).

The paradigm proposed herein is related to computationally efficient information-theoretic BSP in the context of active pose SLAM. We formulate the optimization problem in a topological space where it is more easily solved, yet its solution is close to a solution of the original BSP optimization problem. Unlike existing topological methods in robot motion planning that consider a topology of the configuration space of the robot to generate or topologically classify paths (Kim et al. 2013; Bhattacharya et al. 2015) or to create and use topological maps of the environment (Ranganathan and Dellaert 2011; Choset and Nagatani 2001; Angeli et al. 2009; Paul and Newman 2010; Blöchliger et al. 2018), topology in our work refers to the structure of the belief, which encodes dependency relationships among states, and is used in planning.

An information-theoretic cost in an objective function of BSP is some measure of system uncertainty (information), typically (conditional) entropy or mutual information. Evaluating it requires determining the expected posterior belief of a candidate action. For Gaussian distributions the corresponding computations usually involve calculating a determinant of a posteriori covariance (information) matrix whose complexity is O(n³) in the general case, where n is the state dimension. Moreover, these calculations need to be performed for each candidate action. In Indelman et al. (2015) this challenge was addressed by resorting to the information form and utilizing sparsity; however, calculations still involve expensive access to marginal probability distributions. The rAMDL approach (Kopitkov and Indelman 2017, 2019) performs a one-time calculation of marginal covariance of variables involved in the candidate actions, and then applies an augmented matrix determinant lemma (AMDL) to efficiently evaluate the information-theoretic cost for each candidate action. Nevertheless, that approach still requires recovery of appropriate marginal covariances, the complexity of which depends on state-dimensionality and system sparsity.

Our work is motivated by the results of Khosoussi et al. (2019) in the context of measurement selection and pose-graph pruning problems in SLAM that characterizes the impact of the graph topology of SLAM (described by weighted tree-connectivity) on estimate reliability. In Chen et al. (2021), the authors extend these results to a 3D SLAM with relative pose measurements and make a comparison between the T-optimality and the D-optimality metrics.

1.2. Contributions

The main purpose of this work is to set theoretical foundations of the topological BSP (t-bsp) concept introduced in our preliminary conference publication (Kitanov and Indelman 2018), extending the main results from (Khosoussi et al. 2019) to BSP. In this paper we further develop the potential of t-bsp as an approximate method to efficiently solve BSP in high-dimensional state spaces in problems that exhibit certain structure and develop performance guarantees, as discussed below. Specifically, we consider the structure of pairwise potentials, which is often the case in active SLAM problems. The main contributions of this paper are as follows.

First, we propose new topological t-bsp signatures: one is based on the number of spanning trees of the topological posterior factor graph representation, and the other on its von Neumann graph entropy (Mowshowitz and Dehmer 2012; Passerini and Severini 2009; Petz 2001). Both signatures have a strong correlation with the information-theoretic cost, but are computationally easier to calculate.

Second, we give a definition of an action consistent objective to formalize the equivalence of two criteria (functions) in terms of action ranking which extends previously defined action consistent state approximations proposed in (Elimelech and Indelman 2017b, 2017a, 2017c), building upon (Indelman, 2015, 2016). Definition 1 defines an approximation error in decision-making between any two functions (possibly on different domains) besides of the usual looking at the values of the same function for two different arguments (states or beliefs). This enables us to quantify an error between different cost functions and establish optimality guarantees. Specifically, we express the error of t-bsp via bounds on the information-theoretic cost (Corollary 1), and derive the bounds analytically for an active 2D pose SLAM, considering the joint differential entropy as the cost function. These bounds are a function of topological terms, measurement noise density and an a-priori state estimate. A special emphasis is given to efficiency in calculation of error bounds to make our method appropriate for online use. Using these bounds, we are able to provide online performance guarantees of t-bsp, with any of the topological signatures. We emphasize these performance guarantees are with respect to a given solver (i.e., estimator) of the original information-theoretic BSP problem (6), and not with respect to the corresponding theoretical BSP problem (1). In this work we consider the solver to utilize maximum likelihood observations (Platt et al. 2010), although the bounds are applicable also to additional solvers.

Third, we propose a new method to calculate the number of spanning trees of a graph efficiently via a convergent power series. This is a result of reformulating Kirchoff’s matrix tree theorem via eigenvalues of the normalized Laplacian matrix and graph node degrees (Theorem 1). The power series reveals the connection between our proposed topological signatures and the information-theoretic cost in the limit.

Forth, we develop an incremental variant of t-bsp based on the von Neumann graph entropy that, under certain conditions, which are often met in operating conditions (e.g., SLAM), achieves near constant-time complexity.

We extensively evaluate our batch and incremental t-bsp algorithms in realistic Gazebo/ROS simulations of active 2D pose SLAM and in optimization of synthetic pose graphs. Additionally, we provide a proof of concept of t-bsp in a small-scale real-world active SLAM experiment.

2. Notations and problem formulation

Decision-making under uncertainty can be formulated as a solution to BSP or stochastic control problem where the optimal non-myopic control action ${\mathcal{U}}^{\star }$ at planning time k is found with respect to the objective function J related to the design task as

$\begin{array}{l}{\mathcal{U}}^{\star }=\arg \hspace{0.17em}{\min }_{\mathcal{U}}\hspace{0.17em}J\left(\mathcal{U}\right),\hspace{0.17em}\text{where} \text{for} \mathcal{U}={\mathcal{U}}_{k:k+L-1},\\ J\left(b\left[X_k\right],\mathcal{U}\right)=\underset{{\mathcal{Z}}_{k+1:k+L}}{\mathbb{E}}\left\{{\displaystyle \sum }_{l=0}^{L-1}{c}_l\left(b\left[X_{k+l}\right],{\mathcal{U}}_{k+l}\right)+c_L\left(b\left[X_{k+L}\right]\right)\right\}.\end{array}$

(1)

The BSP problem with Markovian models is an instance of POMDP. The state of the system X is not directly observable by the controller, but through a set of stochastic measurements $\mathcal{Z}$ from which the future posterior beliefs $b\left[X_{k+l}\right]$ must be inferred upon optimization. The expectation in (1) is taken with respect to future (unknown) observations ${\mathcal{Z}}_{k+1:k+L}$ . The optimal solution of BSP provides a control strategy ${\mathcal{U}}^{\star }$ for L look-ahead steps, but L can generally vary among control actions or planning sessions. We use $J\left(\mathcal{U}\right)=J\left(b\left[X_k\right],\mathcal{U}\right)$ and ${\mathcal{U}}^{\star }={\mathcal{U}}_{k:k+L-1}^{\star }$ to denote the objective and the optimal control, respectively, in a given planning session (at time k). The objective function in its general form reflects the design task through immediate cost functions c _l , which depend on the belief evolution $b\left[X_{k+l}\right]$ (to be defined) and a control action ${\mathcal{U}}_{k+l}$ applied at time k + l, and through a final cost c _L . For example, the cost functions can be chosen to minimize trajectory uncertainty, time or energy required to reach a goal, state uncertainty of variables of interest at some specific time instant etc. In information-theoretic BSP, one is interested in state uncertainty minimization which can be expressed through some information-theoretic cost c(·) (see, e.g., Carrillo et al. 2012). This type of cost function is usually computationally the most expensive to optimize in many BSP problems in robotics.

The given notation abstracts the details of the controller design for the purpose of generality of the formulation. The basic problem we aim to solve is evaluating the objective function J, which is belief dependent (POMDP setting), for a finite sequence of controls (L look-ahead steps). This can be used in any POMDP solver no matter whether the control is closed-loop or open-loop. In this paper we implement both open-loop control (action sequences) and a Model Predictive Control (MPC) type planner and assume that a low-level controller can stabilize the system to follow the nominal path between two planning sessions using feedback based on the expected value of the state. This is similar to an LQG approach and is justified for systems that are either linear or locally well approximated by their linearization (Van Den Berg et al. 2011).

Solving the optimization problem (1) explicitly would require belief propagation for a given control and evaluating an information-theoretic objective which we want to avoid and use a topological approach instead. Let us first look at how the belief evolves over time by separating controls and observations to those obtained by planning time k and to future controls and observations after L look-ahead steps.

2.1. Belief propagation

Let $\mathbb{P}\left(X_k|{\mathcal{H}}_k\right)$ represent the posterior probability density function (pdf) at planning time k over states of interest X _k of the robot. In the pose SLAM framework states of interest are the robot’s current and past poses, that is, X _k = {x₀, x₁, …, x _k }. History ${\mathcal{H}}_k\doteq \left\{{\mathcal{Z}}_{1:k},{\mathcal{U}}_{0:k-1}\right\}$ contains all observations ${\mathcal{Z}}_{1:k}$ and controls ${\mathcal{U}}_{0:k-1}$ by time k. Consider conventional state transition and observation models

$x_{i+1}=f\left(x_i,u_i\right)+w_i, z_{i,j}=g\left(x_i,x_j\right)+v_{i,j},$

(2)

with zero-mean Gaussian process and measurement noise $w_i\sim N\left(0,{\mathrm{\Omega }}_w^{-1}\right)$ and $v_{i,j}\sim N\left(0,{\mathrm{\Omega }}_{vij}^{-1}\right)$ , and with known information matrices Ω _w and Ω _vij . The belief $\mathbb{P}\left(X_k|{\mathcal{H}}_k\right)$ can be written as

$\mathbb{P}\left(X_k|{\mathcal{H}}_k\right)\propto \mathbb{P}\left(x_0\right){\displaystyle \prod }_{i=1}^k\mathbb{P}\left(x_i|x_{i-1},u_{i-1}\right)\mathbb{P}\left({\mathcal{Z}}_i|X_i\right).$

(3)

Let the future sampled states of the robot along one of its candidate paths $\mathcal{P}$ generated at planning time k be {x_k+1, …, x_k+L}. The future posterior belief $b\left[X_{k+L}\right]=\mathbb{P}\left(X_{k+L}|{\mathcal{Z}}_{1:k+L},{\mathcal{U}}_{0:k+L-1}\right)$ that would be obtained by following the path $\mathcal{P}$ , can be written in terms of the belief at planning time $\mathbb{P}\left(X_k|{\mathcal{H}}_k\right)$ and the corresponding state transition and observation models as

$b\left[X_{k+L}\right]=\mathbb{P}\left(X_k|{\mathcal{H}}_k\right){\displaystyle \prod }_{l=k+1}^{k+L\left(\mathcal{P}\right)}\mathbb{P}\left(x_l|x_{l-1},u_{l-1}\right)\mathbb{P}\left({\mathcal{Z}}_l|X_l\right),$

(4)

where ${\mathcal{U}}_{k:k+L-1}$ and ${\mathcal{Z}}_{k+1:k+L}$ represent controls and (unknown) observations, respectively, to be acquired by following the path $\mathcal{P}$ .

2.2. Assumptions

The Topological BSP concept is general as an idea of introducing a new cost function, a function of the belief’s topological features, to approximate actions ranking with respect to information-theoretic cost. However, identifying relevant topological representations and signatures in general BSP is an open problem. The approach we develop aims at BSP in problems that exhibit certain structure, such as active SLAM. In this work, our definition of topological space applies to all beliefs which can be factorized to a product of pairwise factors associated with a set of independent measurements. We develop this concept for a special class of Gaussian BSP, targeting efficient decision-making in high-dimensional state spaces, with a joint differential entropy cost function and propose two topological signatures for the case of block isotropic measurement noise (5). For clarity here we only consider the final state cost term $\mathbb{E}\left[c_L\left(b\left[X_{k+L}\right]\right)\right]$ , that is, the joint entropy at the end of the planning horizon. The immediate information-theoretic cost functions can be treated in a similar way.

In summary, the assumptions used in this paper to develop t-bsp are:

(A1) the belief is modeled by a multivariate Gaussian distribution and can be factorized to a product of pairwise factors,

(A2) the measurement noise covariance can be written in the form

$\mathrm{\Sigma }=\text{diag}\left({\sigma }_1^2{I}_m,{\sigma }_2^2{I}_m,\dots ,{\sigma }_{\kappa }^2{I}_m\right),$

(5)

for some κ blocks and m measurements with variances ${\sigma }_1^2,\dots ,{\sigma }_{\kappa }^2$ ,

• (A3) taking maximum likelihood observations (Platt et al. 2010), and

• (A4) the information-theoretic cost is the joint differential entropy at the end of the planning horizon. Under (A3), the objective function becomes

$J\left(\mathcal{U}\right)=N/2\ln \left(2\pi \mathrm{e}\right)-1/2\ln \left|I\left(X_{k+L}\right)\right|,$

(6)

where I(X_k+L) denotes the estimated Fisher information matrix of the robot’s belief b[X_k+L], and N represents the dimension of the state X_k+L.

Note that t-bsp signatures designed to be action consistent under assumption (A4), will also be consistent with respect to an information-gain reward since all actions have the same prior entropy. Specifically, in this paper we demonstrate our approach in an active 2D pose SLAM problem in which all relative positions and orientations are normally distributed with variances ${\sigma }_p^2$ and ${\sigma }_{\theta }^2$ , respectively. Measurement noise covariance Σ then can be written as a diagonal matrix $\mathrm{\Sigma }=\text{diag}\left({\sigma }_p^2{I}_{2m},{\sigma }_{\theta }^2{I}_m\right)$ , with m being the number of relative pose measurements.

Another application of t-bsp are linear systems with relative measurements z_i,j = x _i − x _j + v_i,j such as: active pose SLAM in which the robot’s orientation is known (e.g., from a compass), sensor network localization, clock synchronization and motion consensus (Barooah and Hespanha 2007) in the context of measurement selection problem etc. For those types of problems under the assumptions (A1)-(A4), t-bsp is always action consistent according to the Definition 1 since the determinant of the posterior information matrix is given by $\left|I\left(X_{k+L}\right)\right|=\prod _{p=1}^{\kappa }{\sigma }_p^{-2N}{t}^{\kappa }$ , where t is the number of spanning trees of a topological graph associated to a posterior factor graph, so negative $J\left(\mathcal{U}\right)$ equals the topological signature based on the number of spanning trees.

An extension to a feature-based SLAM and map entropy is possible under the proposed framework as long as the measurements can be expressed in the form of pairwise relations between states. This is quite common since the robot’s landmark measurements are often given relative to the robot’s pose, for example, as range or bearing measurements. The Fisher information matrix of the joint state of feature-based SLAM under the ML framework with isotropic measurement noise is derived in Chen et al. (2018) in terms of its graph topology and our results apply to it.

3. General approach in topological belief space planning

The main idea of topological BSP is ranking candidate actions based on structural information (topology) of their associated beliefs. Under the t-bsp concept, the original problem (1) is reformulated in a topological space, where it can be solved more easily, and then its solution is related to the solution of the original problem. The importance of the topology and a quality of the solution based on it will clearly be dependent on the optimization objective, the influence of non-topological information (noise distribution, geometric realization) and diversity of candidate actions. As will be shown in this work for the information-theoretic BSP, the belief topology is of great importance and can be used to improve efficiency of decision-making.

3.1. Topological representation of beliefs

The structure of the probability density function (pdf) is determined from conditional independences present in it. In this paper, we use a factor graph probabilistic graphical model (Frey et al. 1997) to represent them explicitly. A factor graph defines two types of nodes: the nodes over state variables X, and the factor nodes $F=\left\{f_i\right\}$ such that $\mathbb{P}\left(X\right)=\prod _i{f}_i$ . Variable nodes and factor nodes are independent sets in a bipartite graph. Edges go between factors and variables that those factors depend on. One of the reasons factor graphs are important is the existence of efficient inference algorithms that operate on them, for example, the sum-product message passing algorithm (Kschischang et al. 2001), which allow recursive computation of marginal distributions conditioned on observed data for both Maximum Likelihood (ML) and Maximum-A-Posteriori (MAP) estimation frameworks.

As will be shown later on, factor graphs are also a suitable graphical representation in information-theoretic BSP because of their connection to the objective function under the ML estimation framework for Gaussian systems.

Under assumption (A1), F contains only pairwise factors. The topological representation of the posterior belief is defined as a topological graph G associated to a posterior factor graph FG of a candidate action (e.g., see Figure 2), which is a graph isomorphic to the posterior Markov Random Field (MRF), that is, G ≃ MRF(b[X_k+L]). Different candidate paths $\mathcal{P}$ typically yield different factor graphs. A graph G herein is understood to be simple (without self-loops or parallel edges), connected, and undirected. Each graph G corresponds to a topological space called the geometric realization (Archdeacon 1996). In the standard sense of topology, such a graph is a simplicial 1-complex (Gross and Tucker 1987). Therefore, a node in the graph, a point in its geometric realization and the corresponding point when embedding the graph in a topological space are often used interchangeably.

Figure 2.

Each candidate action corresponds to posterior belief b[X_k+L], that is, trajectory uncertainty (a) which can be represented with a factor graph (b) and assigned a topology. In the case of active pose SLAM, topology can be defined with a simple undirected graph $G=(V, E)$ such that graph nodes V represent robot’s poses and edges E pose constraints between them (c). Topological BSP aims to determine a graph invariant topological signature $s:G\to \mathbb{R}$ which is highly correlated with the information-theoretic cost and maintains action consistent decision-making.

A topological signature $s:G\to \mathbb{R}$ is some graph invariant, that is, a function that assigns to G a number calculated from its structure according to a well-defined procedure, whose value is independent of how the graph is drawn or how its nodes are numbered.

3.2. Decision-making via t-bsp

Generally, we aim to find a topological representation G of the posterior belief corresponding to a control action $\mathcal{U}$ and a function $s:G\to \mathbb{R}$ , which we call topological signature, such that decision-making using the function s does not (or minimally) change the optimal action according to a BSP objective $J\left(\mathcal{U}\right)$ . In other words, suppose we can express the problem of minimizing the objective $J\left(\mathcal{U}\right)$ and its solution ${\mathcal{U}}^{\star }={\mathrm{argmin}}_{\mathcal{U}}J\left(\mathcal{U}\right)$ on the given set/interval of actions $\mathcal{U}$ as maximizing the topological signature s on the same actions set where each action induces some topology, that is,

$\hat{\mathcal{U}}=\underset{\mathcal{U}}{\mathrm{argmax}} s\left(\mathcal{U}\right).$

(7)

We say that decision-making is action consistent if and only if ${\mathcal{U}}^{\star }=\widehat{\mathcal{U}}$ . Moreover, if this is true for any subset of actions, a topological signature s preserves action order too. This is a much stronger condition than action consistency, and as we will see, it will not always be required when we are looking only for the best action, which greatly simplifies design of a topological signature so that we can focus more on computational complexity.

Figure 3 shows all possible situations regarding the relation between the topological signature and objective function in t-bsp. In Figure 3(a), a signature is action consistent on the whole domain of control actions, and therefore the solution of t-bsp is the same as BSP. However, although ordering of actions is not kept in the whole domain in the case shown in Figure 3(b), the solution is still optimal. Figure 3(c) shows a case where an action ordering given by a signature influences the optimal action.

Figure 3.

Different topological signatures and their influence on decision-making. The graphs show topological signature and negative objective as functions of control action.

Clearly, topological signatures should be designed to exhibit strong correlation with the objective, but in the general case, the obtained best action $\widehat{\mathcal{U}}$ may be somewhat different than the optimal action ${\mathcal{U}}^{\star }$ from (1), leading to some error in the quality of solution.

We adopt the definition of action consistent state approximations proposed in Elimelech and Indelman (2017b) and modify it to support t-bsp and action consistent objective approximations in the following way.

Definition 1

Denote by $\widehat{\mathcal{U}}={\mathrm{argmax}}_{\mathcal{U}}s\left(\mathcal{U}\right)$ the optimal solution of t-bsp and by ${\mathcal{U}}^{\star }=\arg \hspace{0.17em}{\min }_{\mathcal{U}}\hspace{0.17em}J\left(\mathcal{U}\right)$ the optimal solution of BSP problem (1). Let $\mathcal{F}$ be a family of monotone decreasing real functions and $f:\mathbb{R}\to \mathbb{R}\in \mathcal{F}$ such that $f\left[s\left(\widehat{\mathcal{U}}\right)\right]=J\left(\widehat{\mathcal{U}}\right)$ and $f=\arg \hspace{0.17em}{\min }_{g\in \mathcal{F}}{\gamma }_g$ , where the functional ${\gamma }_g={\max }_{\mathcal{U}}\left|J\left(\mathcal{U}\right)-g\left[s\left(\mathcal{U}\right)\right]\right|$ . Then, the error of t-bsp is

$ϵ\left(J,s\right)\doteq \left|J\left({\mathcal{U}}^{\star }\right)-f\left[s\left(\widehat{\mathcal{U}}\right)\right]\right|,$

(8)

and γ _f the action ordering error of the signature s.

In particular, ϵ(J, s) = 0 corresponds to t-bsp being action consistent, that is, $\widehat{\mathcal{U}}={\mathcal{U}}^{\star }$ , and when also f exists for which γ _f = 0, simplified representation preserves action order too and we call it action ordering (ranking) consistent. Essentially, f is a mapping from the signature image to the objective function image and is required for defining action consistency of objective functions.

Yet, as in Elimelech and Indelman (2017b), calculating ϵ(J, s) is essentially equivalent to solving the original problem. Therefore, a key aspect will be to provide online performance guarantees by developing bounds on ϵ(J, s) that can be evaluated online. One can then resort to topological BSP to drastically reduce computational cost while carefully monitoring a conservative estimate on the sacrifice in performance, that would be provided by the bound on ϵ(J, s). Another perspective of using this bound is to guarantee global optimality of the anytime t-bsp algorithm we proposed in Kitanov and Indelman (2018). In that approach, actions were ranked by a topological signature and the objective function was evaluated sequentially from best to worst. Having bounds on t-bsp error would provide a stopping condition for action consistent t-bsp, similar to the action elimination scheme proposed in Elimelech and Indelman (2017b) and its application to belief sparsification. The optimal solution is then obtained by evaluating only a subset of candidate actions (for sufficiently tight bounds, empty set), while discarding the rest. Doing so will generally reduce the number of variables for which the marginal covariance needs to be recovered.

4. Information-theoretic topological BSP

We propose two topological signatures for solving information-theoretic BSP with uncertainty of all states considered. Both signatures are based on the spectrum of a graph (normalized) Laplacian associated to the posterior factor graph: a normalized number of spanning trees of a graph, s_ST(G), motivated by the SLAM topological metric (Khosoussi et al. 2015) and adjusted for BSP, and a von Neumann graph entropy, s_VN(G).

A Laplacian matrix of a graph G = (V, E) is by definition L(G) = D(G) − A(G), where A(G) is its |V|×|V| adjacency matrix with elements

$A\left(i,j\right)=\{\begin{array}{l}1,\text{if} \left(i,j\right)\in E\\ 0,\text{otherwise}\end{array}$

and D(G) its node degree matrix defined as a diagonal matrix with graph node degrees on its main diagonal, that is, D(i, i) = d(i) = ∑_j∈V A(i, j). Therefore,

$L\left(i,j\right)\triangleq \{\begin{array}{l}d\left(i\right),\text{if}\hspace{0.17em}i=j\\ -1,\text{if}\hspace{0.17em}i\ne j\hspace{0.17em}\text{and} \left(i,j\right)\in E\\ 0,\text{otherwise}\end{array}$

A normalized Laplacian matrix is defined as $\widehat{L}\left(G\right)=D^{-1/2}L{D}^{-1/2}$ which can be also written in the form

$\widehat{L}\left(i,j\right)=\{\begin{array}{l}1,\text{if} i=j,d\left(i\right)\ne 0\\ \frac{-1}{\sqrt{d\left(i\right)d\left(j\right)}},\left(i,j\right)\in E\\ 0,\text{otherwise}\end{array}$

Both L and $\widehat{L}$ are symmetric and positive semi-definite so all their eigenvalues are real and non-negative. Hence, they can be ordered. For a given connected graph G with n = |V| nodes, let λ₁ ≤ λ₂ ≤ ⋯ ≤ λ _n be the eigenvalues of L, and ${\widehat{\lambda }}_1\le {\widehat{\lambda }}_2\le \cdots \le {\widehat{\lambda }}_n$ , the eigenvalues of $\widehat{L}$ . The smallest eigenvalues ${\lambda }_1={\widehat{\lambda }}_1=0$ . Also, ${\widehat{\lambda }}_n\le 2$ and, since G has no isolated nodes, ${\displaystyle \sum }_{i=1}^n{\widehat{\lambda }}_i=n$ (for proof see Chung 1997).

4.1. Normalized number of spanning trees

The number of spanning trees of a graph t(G) can be determined using Kirchhoff’s Matrix tree theorem (Biggs 1993, Theorem 6.3) which involves calculating any cofactor of a graph Laplacian, or equivalently the product of its non-zero eigenvalues, that is, $t\left(G\right)=\left|\stackrel{\sim }{L}\right|=1/n{\prod }_{i=2}^n{\lambda }_i$ . Here, $\stackrel{\sim }{L}$ denotes L with the r-th row and column removed for some r. Typically, this number gets quite large for factor graphs in BSP so we use its logarithm in a topological signature instead to avoid arithmetic overflow in computer implementations, τ(G) = ln t(G). As has been shown in Khosoussi et al. (2015), there exists a strong positive correlation between D-optimality criterion and τ(G) in general SLAM ML optimization while in some problems with linear measurement models, D-optimality criterion is solely characterized by τ(G). In Kitanov and Indelman (2018), we have proposed a normalization of τ(G) based on its relation to the optimization objective in BSP which gives this BSP topological signature

$s_{ST}\left(G\right)=\frac{\kappa }{2}\ln \hspace{0.17em}t\left(G\right)+\frac{n-1}{2}\left[\ln \left|{\mathrm{\Omega }}_{vij}\right|-\ln {\left(2\pi e\right)}^{\kappa }\right],$

(9)

normalized in such a way to account for different path lengths n and dimension of the robot’s pose κ (e.g., κ = 3 in 2D active pose SLAM) that maintains a high correlation with the information-theoretic cost (6).

Determining s _ST is an operation with cubic complexity in the number of nodes for general dense graphs. Because SLAM graphs typically have a small number of edges due to a limited sensor range and field of view, their graph Laplacian is sparse and state-of-the-art algorithms use this fact to efficiently calculate τ(G) by performing a sparse matrix factorization. For example, let R be the lower triangular Cholesky factor of a reduced Laplacian $\stackrel{\sim }{L}\in {\mathbb{R}}^{n-1\times n-1}$ of graph G, that is, $\stackrel{\sim }{L}=R{R}^T$ , then

$\tau \left(G\right)=\ln \hspace{0.17em}\det \left(\stackrel{\sim }{L}\right)=2{\displaystyle \sum }_{i=1}^{n-1}\ln R\left(i,i\right).$

(10)

This approach, to be efficient, requires finding a good fill-reducing permutation $P_{\stackrel{\sim }{L}}$ of $\stackrel{\sim }{L}$ such that the Cholseky factor of $P_{\stackrel{\sim }{L}}^T\stackrel{\sim }{L}{P}_{\stackrel{\sim }{L}}$ is sparser than the Cholseky factor of $\stackrel{\sim }{L}$ . The problem of finding the best ordering is an NP-complete problem (Yannakakis 1981) and is thus intractable, so heuristic methods are used instead. Overall, the computational time for s _ST depends on the state dimension and system sparsity.

4.2. Von Neumann graph entropy

The second topological signature we propose for information-theoretic BSP is the von Neumann entropy of G, s_VN(G), and its simplification ${\widehat{s}}_{\text{VN}}\left(G\right)$ by node degrees d of G.

The von Neumann entropy s _VN (G) of graph G is the Shannon entropy associated with its normalized Laplacian’s eigenvalues ${\left\{\widehat{{\lambda }_i}\right\}}_{i=1}^n$ and was introduced in Passerini and Severini (2009). In this work we use the following definition

$s_{\text{VN}}\left(G\right)=-{\displaystyle \sum }_{i=1}^n{\widehat{\lambda }}_i/2\hspace{0.17em}\ln \left({\widehat{\lambda }}_i/2\right).$

(11)

Using Han’s quadratic approximation (Han et al. 2012) and given that ${\displaystyle \sum }_{i=1}^n{\widehat{\lambda }}_i^{\hspace{0.17em}k}=\text{Tr}\left[{\widehat{L}}^k\right]$ , it can be simplified to

$\begin{array}{l}{s}_{VN}\left(G\right)\approx \frac{n\hspace{0.17em}\ln \hspace{0.17em}2}{2}-\frac{1}{2}{\displaystyle \sum }_{i=1}^n{\widehat{\lambda }}_i\left({\widehat{\lambda }}_i-1\right)\\ =\frac{n\hspace{0.17em}\ln \hspace{0.17em}2}{2}-\frac{1}{2}\left(\text{Tr}\left[{\widehat{L}}^2\right]-n\right).\end{array}$

(12)

It can be easily seen that $\text{Tr}\left[{\widehat{L}}^2\right]=n+{\displaystyle \sum }_{\left(i,j\right)\in E}\frac{1}{d\left(i\right)d\left(j\right)}$ from which the final expression for our approximated von Neumann graph entropy follows which we use as a topological signature in BSP

$s_{\text{VN}}\left(G\right)\approx {\widehat{s}}_{\text{VN}}\left(G\right)=n/2\hspace{0.17em}\ln \hspace{0.17em}2-\frac{1}{2}{\displaystyle \sum }_{\left(i,j\right)\in E}\frac{1}{d\left(i\right)d\left(j\right)}.$

(13)

Notice that its computation depends only on graph node degrees and, in the general case, has quadratic complexity in the number of nodes, O(n²) but in the case of BSP, where $\widehat{L}$ is sparse, it depends only on the small number of non-zero elements of A(G), that is, the number of edges |E| in the graph G. Also, the expression (13) can be computed incrementally, as new edges (measurements) are added to the factor graph as the robot explores the environment or re-plans its path. This effectively improves computational complexity of the t-bsp algorithm to nearly O(1) since the number of measurements between two planning sessions is limited.

4.2.1. Incremental mode

Let G _k = (V _k , E _k ) be a topological graph of a prior belief b[X _k ], a prior graph at planning time k for short, and G_k+L = (V_k+L, E_k+L) the posterior topological graph corresponding to b[X_k+L] after obtaining future predicted observations associated to a control action $\mathcal{U}$ , a posterior graph for short. Their corresponding graph signatures, ${\widehat{s}}_{VN}\left(G_k\right)$ and ${\widehat{s}}_{VN}\left(G_{k+L}\right)$ , are

$${\widehat{s}}_{\text{VN}}\left(G_k\right)=\left(k+1\right)/2\ln \hspace{0.17em}2-1/2\stackrel{q_k}{\overbrace{{\displaystyle {\sum }_{\left(i,j\right)\in E_k}1}/\left[d_k\left(i\right)d_k\left(j\right)\right]}}$$

(14)

$${\widehat{s}}_{\text{VN}}\left(G_{k+L}\right)=\left(k+L+1\right)/2\ln 2-1/2\stackrel{q_{k+L}}{\overbrace{{\sum }_{\left(i,j\right)\in E_{k+L}}1/\left[d_{k+L}\left(i\right)d_{k+L}\left(j\right)\right]}}$$

(15)

By subtracting equation (14) from (15) we obtain the recursive update rule for the posterior graph signature

${\hat{s}}_{VN}\left(G_{k+L}\right)={\hat{s}}_{VN}\left(G_k\right)+\frac{L}{2}\ln \hspace{0.17em}2-\frac{1}{2}\mathrm{\Delta }q,\text{where}$

(16)

$\mathrm{\Delta }q=q_{k+L}-q_k.$

(17)

Now, the key observation is that in order to calculate Δq we do not need to iterate over all edges of the corresponding graphs, but only over those whose node degrees changed or are associated to the new states added to the prior graph, that is,

$\begin{array}{l}\mathrm{\Delta }q={\displaystyle \sum }_{\left(i,j\right)\in E_{\delta }}\left(\frac{1}{d_{k+L}\left(i\right)d_{k+L}\left(j\right)}-\frac{1}{d_k\left(i\right)d_k\left(j\right)}\right)\\ +{\displaystyle \sum }_{\left(i,j\right)\in \mathrm{\Delta }E}\frac{1}{d_{k+L}\left(i\right)d_{k+L}\left(j\right)}.\end{array}$

(18)

In equation (18), ΔE = E_k+L − E _k represents newly added edges to the prior graph G _k due to the control action $\mathcal{U}$ yielding posterior graph G_k+L, V _I nodes incident to ΔE without new nodes and E _δ edges from E_k+L incident with V _I that belong also to E _k . An example illustrating the incremental t-bsp concept is given in Figure 4.

Figure 4.

Incremental t-bsp: Consider the planning session starting at time k=4 when the robot’s current pose is x₄, and a control action that generates a candidate path from which states $\left\{x_5,x_6\right\}$ (marked with red circles) are sampled and added to the prior graph together with new edges $\mathrm{\Delta }E=\left\{\left(x_4,x_5\right),\left(x_5,x_6\right),\left(x_5,x_3\right)\right\}$ marked with blue dashed lines as a result of predicted relative pose measurements. In that case, involved states are $V_I=\left\{x_3,x_4\right\}$ (marked with blue circles), and affected edges in the prior graph $E_{\delta }=\left\{\left(x_2,x_3\right),\left(x_3,x_4\right),\left(x_2,x_4\right)\right\}$ .

The complexity of updating ${\widehat{s}}_{VN}$ graph signature per candidate action is O(|ΔE| + |MB(V _I )|), where MB(V _I ) denotes the Markov blanket in G _k of the variables involved and is usually bounded owing to limits on sensor range. Since the number of actions per planning session is also bounded, for sparse graphs this means that incremental t-bsp algorithm has effective complexity O(1).

4.3. Maximum likelihood estimation

In this work, we investigate error bounds of t-bsp on the active 2D pose SLAM problem. We first examine results on passive SLAM reliability regarding its graph structure described by a topological metric τ(G)≐ln(t(G)) proposed in Khosoussi et al. (2015) and then, in the next section, we show how these results can be extended to a BSP problem to provide online performance guarantees of t-bsp. In that section, it will also be clear how we chose the topological signature s _ST .

In the context of Maximum Likelihood Estimation (MLE) in pose SLAM, the optimal set of poses X _k for which the belief (3) is maximized can be obtained by fixing one of the poses, for example, x₀, and treating the rest as unknown (or with an uninformative prior) while minimizing the sum of weighted squared errors between predicted and measured relative poses, ¹ that is,

${\widehat{X}}_k=\underset{X_k}{\mathrm{argmax}}\mathbb{P}\left({\mathrm{\Delta }}_k|X_k\right)\equiv \underset{X_k}{\arg \hspace{0.17em}\min }\Vert {\mathrm{\Delta }}_k-h\left(X_k\right){\Vert }_{{\mathrm{\Sigma }}^{-1}}^2.$

(19)

In this formulation, a measurement Δ _k represents a vector of m stacked relative pose measurements $z_{i,j}^r\in \text{SE}\left(2\right),r=\mathrm{1,2},\dots ,m$ from motion and observation model (2) at time k with m = |E|, the number of edges in the topological graph of the belief (3). Relative pose measurements in pose SLAM resulting from state transitions can be obtained by the motion composition z_i+1,i(x _i , x_i+1) = ⊖x _i ⊕ x_i+1 = ⊖x _i ⊕ f(x _i , u _i , w _i ). In this work, we assume independent relative pose measurements with additive noises

${\mathrm{\Delta }}_k=h\left(X_k\right)+{\nu }_k,{\nu }_k\sim N\left(0,{\mathrm{\Sigma }}^{-1}\right).$

(20)

For simplicity, we also assume a 2D pose SLAM setting in which all relative positions and orientations between poses x _i and x _j have equal variance, ${\sigma }_p^2$ and ${\sigma }_{\theta }^2$ , respectively, that is, ${\mathrm{\Omega }}_{vij}=\text{diag}\left({\sigma }_p^{-2},{\sigma }_p^{-2},{\sigma }_{\theta }^{-2}\right)$ . Measurement noise covariance Σ in that case can be written as a diagonal matrix $\mathrm{\Sigma }=\text{diag}\left({\sigma }_p^2{I}_{2m},{\sigma }_{\theta }^2{I}_m\right)$ by reordering elements of Δ _k .

The information matrix I(X _k ) of the MLE is I(X _k ) = H ^T Σ⁻¹H (Sorenson 1980), where H = ∂h/∂X _k . is a measurement Jacobian. I(X _k ) evaluated at the true value of X _k is known as Fisher Information Matrix (FIM) and its inverse the Cramér-Rao lower bound (CRLB). Commonly, FIM is approximated with $I\left({\widehat{X}}_k\right)$ . In Khosoussi et al. (2015) bounds of the determinant of I(X _k ) are expressed in terms of pose SLAM graph topology, geometry and noise as

$\begin{array}{r}3\tau \left(G\right)+\ln \hspace{0.17em}\det I_o\left(X_k\right)\le \ln \hspace{0.17em}\det \hspace{0.17em}I\left(X_k\right)\\ \hspace{0.17em}\le \hspace{0.17em}2\tau \left(G\right)+\ln \hspace{0.17em}\det \left(\tilde{L}+\mathrm{\Psi }I\right)+\ln \hspace{0.17em}\det I_o\left(X_k\right),\end{array}$

(21)

where $\stackrel{\sim }{L}$ is a reduced Laplacian of a graph G obtained by removing an arbitrary r-th row and r-th column of the graph Laplacian L, I _o (X _k ) estimated information matrix based on the odometry measurements Δ _k = {z_i+1,i, i < k}, and $\mathrm{\Psi }\doteq {\xi }^2{\text{dist}}_{\max }^2$ where ξ = σ _θ / σ _p , and ${\text{dist}}_{\max }^2={\max }_{i\in V}{\sum }_{\left(i,j\right)\in E}{\Vert x_i-x_j\Vert }^2$ . Notice that generally Ψ depends on the noise variances, geometry, and topology of the SLAM graph. In Khosoussi et al. (2015) these bounds are not used except to prove the limiting case, Ψ → 0. Therefore, for small values of Ψ a good approximation of ln detI(X _k ) is its bound that depends on τ(G), that is, lower and upper bounds become tight. Even when it is not negligible, if there is only one path realization to consider as in passive SLAM, information gain of adding relative pose measurements with a constant noise distribution to a SLAM odometry graph is solely characterized by graph G topology, that is, Ψ = Ψ(G). Similar logic applies to graph pruning and the measurement selection problem (Khosoussi et al. 2019) where all graphs are only subgraphs of the original graph with the same embedding in metric space. To demonstrate why we cannot use the same metric τ(G) in BSP problems nor guarantee optimality using the bounds on I(X _k ), consider the example given in Figure 5.

Figure 5.

BSP vs. the measurement selection problem: In measurement selection problems, the robot considers a single path and its factor graph (e.g., marked with blue color) at planning time k and which subset of measurements (marked with blue dashed lines) to take. Pose samples X_k+L are fixed and minimizing entropy (6) by a measurements subset (action) is the same as maximizing ln |I(X_k+L)| − ln |I _o (X_k+L)|. Its bounds as can be seen from (21) depend only on the topology and ξ, that is, all actions share the same pose variables X_k+L which can be considered constant in optimization. However, in BSP where a robot needs to compare different paths $\mathcal{U}$ corresponding to different factor graphs (e.g., red and green), this is not true anymore. To determine the best action according to (6), one has to account additionally for different path geometries and path lengths and larger variety of topologies.

So, in BSP we need to consider different path realizations X_k+L and therefore $\mathrm{\Psi }=\mathrm{\Psi }\left(\mathcal{U}\right)=\mathrm{\Psi }\left(X_{k+L}\left(\mathcal{U}\right),G\left(\mathcal{U}\right)\right)$ , in contrast to graph pruning and measurement selection. Notice that we do not need to propagate belief in planning for determining Ψ under the ML observations assumption, that is, $\mathbb{E}\left[X_{k+L}\right]=\left[{\widehat{X}}_k{x}_{k+1}\dots x_{k+L}\right]$ where future sampled poses from the path corresponding to action $\mathcal{U}$ are added to the prior state estimate ${\widehat{X}}_k$ which is the same for all actions.

4.4. Entropy bounds for active SLAM

In BSP, we have to consider multiple path realizations from different controls, with greater variety in both topology of factor graphs and other non-topological factors that influence estimation accuracy, for example, non-fixed geometry and different path lengths. In Kitanov and Indelman (2018), we developed a topological signature s _ST (9) for t-bsp. Here, we show its derivation and provide global optimality guarantees of t-bsp for any signature via estimated entropy bounds that are functions of s _ST for active SLAM and related problems as described in the Section 2.2.

For a control action $\mathcal{U}$ corresponding to robot’s poses X_k+L = {x₁, …, x_k+L} each of dimension κ, the entropy of the future posterior belief b[X_k+L] can be written using equation (6) as

$J\left(\mathcal{U}\right)=\left(k+L\right)\kappa /2\ln \left(2\pi \mathrm{e}\right)-1/2\ln \left|I\left(X_{k+L}\right)\right|$

(22)

Notice that the number of graph nodes |V| = n = k + L + 1. Using Lemma 2 and Lemma 3 from Khosoussi et al. (2015), for posterior belief $\mathbb{P}\left(X_{k+L}|{\mathcal{U}}_{0:k+L-1}\right)$ corresponding to an odometry factor graph whose topological graph G ^o is a tree, it follows

$\begin{array}{ll}\ln \hspace{0.17em}\det I_o\left(X_{k+L}\right)& =\underset{0}{\underbrace{\tau \left(G^o\right)}}+\ln {\left({\sigma }_p^{-4}{\sigma }_{\theta }^{-2}\right)}^{k+L}\\ & =\left(k+L\right)\ln \hspace{0.17em}\det \left({\mathrm{\Omega }}_{{\nu }_{i,j}}\right)\end{array}$

(23)

Inequalities (21) and equations (22) and (23) give the entropy bounds $\mathcal{L}\mathcal{B}\left[J\left(\mathcal{U}\right)\right]⩽J\left(\mathcal{U}\right)⩽\mathcal{U}\mathcal{B}\left[J\left(\mathcal{U}\right)\right],\text{where}$

$\begin{array}{l}\mathcal{U}\mathcal{B}\left[J\left(\mathcal{U}\right)\right]=\frac{\left(n-1\right)\kappa }{2}\ln \left(2\mathrm{\pi }e\right)-\frac{1}{2}\left[3\tau \left(G\right)+\left(n-1\right)\ln \left|{\mathrm{\Omega }}_{{\nu }_{i,j}}\right|\right]\\ =-\frac{3}{2}\tau \left(G\right)-\frac{n-1}{2}\left[\ln \left|{\mathrm{\Omega }}_{{\nu }_{i,j}}\right|-\ln {\left(2\mathrm{\pi }e\right)}^{\kappa }\right]=-s_{ST}\left(\mathcal{U}\right)\end{array}$

(24)

Similarly, for the lower bound we get

$\begin{array}{ll}\mathcal{L}\mathcal{B}\left[J\left(\mathcal{U}\right)\right]& =-s_{ST}\left(\mathcal{U}\right)-1/2\hspace{0.17em}\ln \left|\stackrel{\sim }{L}+\mathrm{\Psi }\left(\mathcal{U}\right)I_{n-1}\right|+1/2\tau \left(G\right)\end{array}$

(25)

$=-s_{ST}\left(\mathcal{U}\right)-1/2\left[\ln \left|\stackrel{\sim }{L}+\mathrm{\Psi }\left(\mathcal{U}\right)I_{n-1}\right|-\ln \left|\stackrel{\sim }{L}\right|\right]$

(26)

$=-s_{ST}\left(\mathcal{U}\right)-1/2\hspace{0.17em}\ln \left|I_{n-1}+{\stackrel{\sim }{L}}^{-1}\mathrm{\Psi }\left(\mathcal{U}\right)\right|.$

(27)

In equation (26) we used the Matrix tree theorem that states $t\left(G\right)=\left|\stackrel{\sim }{L}\right|$ (recall τ(G) = ln t(G)), and in equation (27) Shur’s determinant lemma. From equation (27) we see that when Ψ → 0, the entropy goes to the BSP topological signature and solely depends on the belief’s topology and path length. Otherwise, we have to account for the graph embedding in the metric space as well, as it appears in the scalar function Ψ of the second term in the above equations. Now, given all candidate control actions, the following corollary provides the bounds on the t-bsp error as defined by Definition 1.

Corollary 1

For the error ϵ(J, s) of t-bsp, we can write

$ϵ\left(J,s\right)⩽\mathrm{\Delta }{J}_{\mathit{max}},$

where $$\begin{gathered} \mathrm{\Delta }{J}_{\mathit{max}}=\mathcal{U}\mathcal{B}\left[J(\hat{\mathcal{U}})\right]-\underset{\mathcal{U}}{\min }\mathcal{L}\mathcal{B}\left[J\left(\mathcal{U}\right)\right], and\hspace{0.17em} \\ \hat{\mathcal{U}}={\mathrm{argmax}}_{\mathcal{U}}s\left(\mathcal{U}\right) \end{gathered}$$ .

Proof

See Appendix B.

4.4.1. Efficient calculation of entropy bounds

The upper bound of the entropy is already determined by the topological signature s _ST as can be seen from equation (24). Moreover, $\mathcal{U}\mathcal{B}\left[J\left(\widehat{\mathcal{U}}\right)\right]=-s_{ST}\left(\widehat{\mathcal{U}}\right)$ is the minimum upper bound of all actions since we select action in t-bsp according to equation (7). However, calculating the lower bound requires an additional cost due to the second term in equations (25)-(27). Calculating (25)-(26) requires evaluating the determinant of a sparse matrix $M=\stackrel{\sim }{L}+\mathrm{\Psi }{I}_{n-1}\in {\mathbb{R}}^{n-1\times n-1}$ , whereas calculating (27) is even more complex because it requires inverting $\stackrel{\sim }{L}$ and finding a determinant of a dense matrix.

Another idea is to find some fast method for limiting the determinant of M from above to replace the second term in equation (25) that will not introduce a big difference in tightness of the lower bound.

4.4.2. Exact lower bound

A direct approach performs from scratch some sparse matrix factorization of M, for example, Cholesky factorization M = RR ^T where R is the lower triangular matrix, from which then it is easy to calculate its determinant. However, this approach, to be efficient, still requires finding a good fill-reducing permutation P _M of M, but we notice that some calculations from the topological signature can be re-used. In particular, since M differs from $\stackrel{\sim }{L}$ only in diagonal elements, they both have the same sparsity pattern. Therefore, if $P_{\stackrel{\sim }{L}}$ is the best fill-reducing permutation of $\stackrel{\sim }{L}$ (already found for determining $\left|\stackrel{\sim }{L}\right|$ ), it can be re-used for calculation of the lower bound, that is, $P_M=P_{\stackrel{\sim }{L}}$ .

4.4.3. Hadamard bound

Since $\stackrel{\sim }{L}$ is a reduced graph Laplacian, it is symmetric positive definite (SPD), and because also Ψ > 0, the matrix M is SPD. For large values of Ψ, the matrix M becomes diagonally dominant, in fact strongly so, and Hadamard inequality gives a good approximation of its determinant, that is,

$\left|\stackrel{\sim }{L}+\mathrm{\Psi }{I}_{n-1}\right|⩽{\displaystyle \prod }_{i=2}^n[d\left(i\right)+\mathrm{\Psi }].$

(28)

Calculation of the right side of inequality (28) requires only multiplication of node degrees with added value of Ψ. Applying (28) to equation (25), we can get a somewhat more conservative but faster to compute lower bound

$\mathcal{L}{\mathcal{B}}_H\left[J\left(\mathcal{U}\right)\right]=-s_{\text{ST}}\left(\mathcal{U}\right)+\frac{1}{2}\left(\tau \left(\mathcal{U}\right)-{\displaystyle \sum }_{i=2}^n\ln \left[d_{\mathcal{U}}\left(i\right)+\mathrm{\Psi }\left(\mathcal{U}\right)\right]\right).$

(29)

Algorithm 1. t-bsp with performance guarantees

4.5. t-bsp algorithm

Algorithm 1 summarizes our proposed method and highlights its possible uses in BSP regarding desired performance specifications. Performance guarantees can be either in the form of selected solution’s entropy upper bound, that is, guarantees on the accuracy, or in bounding the error of t-bsp with respect to the optimal solution.

The first form can be used when the maximum admissible path uncertainty is known at planning time, for example, for obstacle avoidance. In that case, one can get an answer if a t-bsp solution satisfies the specification by ranking actions using the very efficient topological signature ${\widehat{s}}_{VN}$ and calculating only its entropy’s upper bound (Alg. 1, line 7). If global optimality guarantees are required, the lower entropy bound needs to be calculated for all actions. Although in equation (29) it is given as a function of s _ST , in the next section we will see how it can be expressed via ${\widehat{s}}_{VN}$ . In the first usage, if uncertainty specification is not met by t-bsp one can still use its ranked actions set A in anytime algorithm as we proposed in Kitanov and Indelman (2018). Also, if an optimal solution needs to be found, we can use t-bsp to eliminate suboptimal actions.

5. Von Neumann entropy based t-bsp with performance guarantees

In this section we develop a mechanism to provide global optimality guarantees as functions of the von Neumann entropy topological signature ${\widehat{s}}_{VN}$ (13). The key idea is to make a connection of ${\widehat{s}}_{VN}$ to the normalized number of spanning trees topological signature s _ST based on which we have already established t-bsp error bounds (see Figure 6). At the same time, we want to preserve computational efficiency. Theorem 1 is central for this, and has also much wider applicability than in the context of BSP as a reformulation of the Matrix tree theorem.

Figure 6.

Schematic overview: Topological belief space planning based on approx. von Neumann entropy with performance guarantees.

5.1. Taylor series for τ(G)

First, we provide a method for calculating the number of spanning trees of a graph via a convergent Taylor series expansion as a function of only the graph’s node degrees, which avoids matrix factorization, the current state-of-the-art.

Recall that $\tau \left(G\right)={\sum }_{i=2}^n\ln \hspace{0.17em}{\lambda }_i-\ln \hspace{0.17em}n$ and representing it with a Taylor power series is possible only for all ${\lambda }_i\in \mathfrak{I}$ inside the interval of convergence of the logarithm function, $\mathfrak{I}=\hspace{0.17em}(0,2]$ around point λ_i0 = 1. However, the spectrum of the graph Laplacian can be outside this interval. We know that the largest Laplacian eigenvalue is bounded by the number of non-isolated vertices, that is, λ _n ⩽n (Mohar 1991, Theorem 2.2). On the other hand, the normalized graph Laplacian’s spectrum without zero lies inside $\mathfrak{I}$ for every graph G (Wilson and Zhu 2008) and therefore the von Neumann graph entropy is well-defined via its Taylor series. This motivates us to look for an alternative representation of τ(G) that involves eigenvalues of $\widehat{L}$ . The following theorem gives the required relation.

Theorem 1

Let G = (V, E) be a connected, undirected graph with n nodes, and let t(G) be the number of spanning trees of G. Then,

$t\left(G\right)=\frac{{\prod }_{i=1}^n{d}_i}{{\sum }_{i=1}^n{d}_i}{\displaystyle \prod }_{i=2}^n{\widehat{\lambda }}_i$

(30)

where ${\widehat{\lambda }}_2,{\widehat{\lambda }}_3,\dots ,{\widehat{\lambda }}_n$ are non-zero eigenvalues of the normalized Laplacian matrix $\widehat{L}$ of G, and d₁, d₂…, d _n node degrees of G.

Proof. See Appendix C.

We will often use the logarithm of t(G) in the following study

$\mathrm{\tau }\left(G\right)\doteq \ln \hspace{0.17em}t\left(G\right)=\ln \frac{{{\displaystyle \prod }}_{i=1}^n{d}_i}{{{\displaystyle \sum }}_{i=1}^n{d}_i}+{\displaystyle \sum }_{i=2}^n\ln {\widehat{\lambda }}_i.$

(31)

Let us denote by $T_{x_0}\left(x\right)$ the Taylor series expansion of the function f(x) = ln x centered around the point x₀. Then, on the interval of convergence $\mathfrak{I}$ , $f\left(x\right)=T_{x_0}\left(x\right)=\underset{r\to \infty }{\lim }{T}_{x_0}^r\left(x\right)$ where the r-th partial sum of the Taylor series $T_{x_0}^r\left(x\right)$ is given by

$T_{x_0}^r\left(x\right)=\ln \hspace{0.17em}{x}_0+{\displaystyle \sum }_{i=1}^r\frac{{\left(-1\right)}^{i-1}}{i{x}_0^i}{\left(x-x_0\right)}^i.$

Considering function f defined over the non-zero normalized graph Laplacian’s eigenvalues ${\widehat{\lambda }}_i$ and the linearization point x₀ = 1, we obtain the following r-th order Taylor series approximation of $\ln {\hat{\lambda }}_i$

$\begin{array}{l}\ln \hspace{0.17em}{\widehat{\lambda }}_i\approx T_1^r\left({\widehat{\lambda }}_i\right)=\\ {\displaystyle \sum }_{k=1}^r\frac{{\left(-1\right)}^{k-1}}{k}{\left({\widehat{\lambda }}_i-1\right)}^k,\forall {\widehat{\lambda }}_i\in \text{spectrum}\left(\widehat{L}\left(G\right)\right)\setminus \left\{0\right\}.\end{array}$

(32)

Calculating the topological signature s _ST directly using the Matrix tree theorem, using Theorem 1 or via a Taylor series approximation using equation (32) would require determining the graph spectrum of the associated graph matrix, which is not desirable, from a computational perspective, in high-dimensional state spaces. Similarly, applying equation (10) requires matrix factorization, whose time complexity is only by a constant scaling factor smaller than the time complexity of calculating the information-theoretic cost.

In this paper, we show how this can be avoided by developing the power series of τ(G) and expressing it with sums of traces of powers of normalized graph Laplacian which can be more easily calculated because they involve multiplication of sparse matrices. Importantly, for low order Taylor series approximations, they can be given in closed-form as simple functions of graph node degrees.

Denote the k-th term in the sum given by equation (32) by

$s_k\left({\hat{\lambda }}_i\right)\doteq \frac{{\left(-1\right)}^{k-1}}{k}{\left({\hat{\lambda }}_i-1\right)}^k.$

After expanding a power using Binomial theorem we can write

$s_k\left({\widehat{\lambda }}_i\right)=\underset{l=0}{{\displaystyle \sum }^k}\underset{a_{kl}}{\underbrace{\frac{{\left(-1\right)}^{k+l-1}}{k}\left(\begin{array}{l}k\\ l\end{array}\right)}}{\widehat{\lambda }}_i^{k-l},\text{so}$

(33)

$T_1^r\left({\widehat{\lambda }}_i\right)={\displaystyle \sum }_{k=1}^r\underset{l=0}{{\displaystyle \sum }^k}{a}_{kl}{\widehat{\lambda }}_i^{k-l}.$

(34)

Observe that in equation (34), a _kl is defined by the horizontal underbrace in equation (33).

By convention ${\widehat{\lambda }}_1^0=0$ and given that ${\sum }_{i=2}^n{\widehat{\lambda }}_i^{k-l}={\sum }_{i=1}^n{\widehat{\lambda }}_i^{k-l}=\text{Tr}\left[{\widehat{L}}^{k-l}\right]\Rightarrow$

$\begin{array}{l}\sum _{i=2}^n{T}_1^r\left({\hat{\lambda }}_i\right)=\underset{i=1}{\sum ^n}\sum _{k=1}^r\underset{l=0}{\sum ^k}{a}_{kl}{\hat{\lambda }}_i^{k-l}=\sum _{k=1}^r\underset{l=0}{\sum ^k}{a}_{kl}\underset{i=1}{\sum ^n}{\hat{\lambda }}_i^{k-l}\\ =\sum _{k=1}^r\underset{l=0}{\sum ^k}{a}_{kl}\mathrm{T}\mathrm{r}\left[{\hat{L}}^{k-l}\right].\end{array}$

(35)

For the Taylor polynomial $T_1^r\left({\widehat{\lambda }}_i\right)$ , $\exists \zeta =\zeta \left({\widehat{\lambda }}_i\right)$ such that a remainder term $R_1^r\left({\widehat{\lambda }}_i\right)=f\left({\widehat{\lambda }}_i\right)-T_1^r\left({\widehat{\lambda }}_i\right)$ is

$R_1^r\left({\widehat{\lambda }}_i\right)=\frac{f^{\left(r+1\right)}\left(\zeta \right)}{\left(r+1\right)!}{\left({\widehat{\lambda }}_i-1\right)}^{r+1}=\frac{{\left(-1\right)}^r}{r+1}{\left(\frac{{\widehat{\lambda }}_i-1}{\zeta }\right)}^{r+1},$

(36)

where for ${\widehat{\lambda }}_i<1,\zeta \in \left[{\widehat{\lambda }}_i,1\right]$ and for ${\widehat{\lambda }}_i⩾1,\zeta \in \left[1,{\widehat{\lambda }}_i\right]$ .

Finally, using Theorem 1 and equation (35) we obtain

$\widehat{\tau }\left(G,r\right)=\ln \frac{\prod d_i}{\sum d_i}+{\displaystyle \sum }_{k=1}^r\underset{l=0}{{\displaystyle \sum }^k}{a}_{kl}\mathrm{T}\mathrm{r}\left[{\widehat{L}}^{k-l}\right].$

(37)

$\widehat{\tau }\left(G,r\right)$ is the approximation of τ(G) by the r-th order Taylor polynomial and can be used to calculate a topological signature of t-bsp with reduced complexity compared to calculating it by matrix factorization as in (10), by an operation which involves sparse matrix multiplications instead.

5.2. Performance guarantees based on $\widehat{\tau }$

The next proposition reveals a property of the remainder term which can be used to provide performance guarantees of t-bsp efficiently, from the approximated number of spanning trees of the topological graph by its Taylor polynomial, without the need to know the remainder of all, but only the selected control action.

Proposition 1

If the logarithm of the number of spanning trees of the graph G is approximated by the r-th order Taylor polynomial $\widehat{\tau }\left(G,r\right)=\ln \hspace{0.17em}(\prod d_i/\sum d_i)+{\displaystyle \sum }_{k=1}^r\underset{l=0}{{\displaystyle \sum }^k}{a}_{kl}\mathrm{T}\mathrm{r}\left[{\widehat{L}}^{k-l}\right]$ , its approximation error $\epsilon \left(G,r\right)=\tau \left(G\right)-\widehat{\tau }\left(G,r\right)$ has the following properties:

i. $\epsilon \left(G,r\right)={\sum }_{i=2}^n{R}_1^r\left({\widehat{\lambda }}_i\right)$ , where $R_1^r\left({\widehat{\lambda }}_i\right)$ is given by equation (36),

ii. lim_r→∞ ɛ(G, r) = 0,

iii. ɛ(G, r) ≤ 0 for odd polynomial orders r.

Proof

See Appendix D.

Figure 7 illustrates the properties of the remainder stated by the Proposition 1.

Figure 7.

Taylor series approximation error $\tau -\widehat{\tau }$ of the logarithm of the number of spanning trees for three different graphs: isosahedral, antenna, and Foster026. For odd polynomial orders, this error is either zero or negative.

The question that naturally arises is whether we can use the approximated logarithm of the number of spanning trees to provide performance guarantees if we choose such an order r that makes $\widehat{\tau }\left(G,r\right)\ge \tau \left(G\right)$ . From equation (29) it is easy to see that the lower bound of the entropy calculated from $\widehat{\tau }\left(G,r\right)$ will remain the lower bound, but the upper bound might be compromised depending on the approximation error ɛ(G, r). So, ΔJ_max needs to account for that change as well. Let’s denote by $\mathrm{\Delta }{\widehat{J}}_{\max }$ the t-bsp error bound calculated by using $\widehat{\tau }\left(G,r\right)$ instead of the true τ(G). Then,

$\mathrm{\Delta }{J}_{max}=\mathrm{\Delta }{\widehat{J}}_{max}-\kappa /2\epsilon \left(\widehat{\mathcal{U}},r\right),$

(38)

where $\epsilon \left(\widehat{\mathcal{U}},r\right)=\tau \left(\widehat{\mathcal{U}}\right)-\widehat{\tau }\left(\widehat{\mathcal{U}},r\right)$ .

5.3. Relation between topological signatures

By establishing a relation between s _ST and ${\widehat{s}}_{VN}$ it would be possible to use benefits from both worlds, speed of s _VN with optimality guarantees for s _ST .

A relation between two proposed information-theoretic BSP topological signatures s _ST and ${\widehat{s}}_{VN}\left(G\right)$ can be derived in the limit r → ∞ using Theorem 1 and Taylor series expansion of the logarithms of the normalized graph Laplacian’s eigenvalues, which appear in both of them (see Appendix E):

$s_{ST}\left(G\right)=\underset{r\to \infty }{\lim }{s}_{ST}\left(\widehat{\tau }\left(G,r\right)\right),\text{where}$

(39)

$\begin{array}{l}{s}_{ST}\left(\widehat{\tau }\left(G,r\right)\right)=\frac{\kappa r\left(r-1\right)}{4}{\widehat{s}}_{VN}\left(G\right)+\\ \frac{\kappa }{2}\left({\displaystyle \sum }_{k=3}^r{\displaystyle \sum }_{l=0}^{k-3}{a}_{kl}Tr\left[{\widehat{L}}^{k-l}\right]+\ln \frac{\prod d_i}{\sum d_i}\right)+\\ \left[\frac{\kappa }{2}\left(r-\frac{r\left(r-1\right)\left(\ln \hspace{0.17em}2+1\right)}{4}-H\left(r\right)\right)+\frac{1}{2}\ln \frac{\left|{\mathrm{\Omega }}_{vij}\right|}{{\left(2\mathrm{\pi }e\right)}^{\kappa }}\right]n+\\ \frac{\kappa }{2}H\left(r\right)-\frac{1}{2}\ln \frac{\left|{\mathrm{\Omega }}_{vij}\right|}{{\left(2\pi e\right)}^{\kappa }},\text{and}\end{array}$

(40)

$H\left(r\right)={\sum }_{k=1}^{r}1/k$ is a harmonic number of r. In practice, we can often get very good approximation for finite small orders r.

For r = 3, using equation (61) we find a simple closed-form expression given by equation (41), that does not involve traces of matrix powers, and in combination with Proposition 1 gives a way to formulate t-bsp in terms of the approximated von Neumann entropy and lower bound the information-theoretic cost for all actions $\mathcal{U}$ .

For the logarithm of the number of spanning trees of a graph G corresponding to a control action $\mathcal{U}$ approximated by the third order polynomial we can write

$\begin{array}{l}\widehat{\tau }\left(G,3\right)=\ln \frac{\mathrm{\Pi }{d}_i}{\mathrm{\Sigma }{d}_i}+\frac{7}{3}\left({\widehat{s}}_{\text{VN}}\left(G\right)-\frac{n\hspace{0.17em}\ln \hspace{0.17em}2}{2}\right)+\\ \frac{1}{3}{\displaystyle \sum }_{\left(i,k\right)\in E}{\widehat{l}}_{ik}\left({\displaystyle \sum }_j{\widehat{l}}_{ij}{\widehat{l}}_{jk}\right)+\frac{11}{6}\end{array}.$

(41)

In equation (41) $\widehat{L}=\left(\widehat{l_{ij}}\right)$ which means that the right-hand side is purely a function of nodes’ degrees and a number of graph nodes, both of which are easy to maintain for graph operations of adding or deleting an edge encountered in BSP or graph pruning problems.

Substituting (41) into (9), we obtain the third order relation between the topological signatures

$\begin{array}{l}{\widehat{s}}_{\text{ST}}\left(G,3\right)=\frac{7\kappa }{6}{\widehat{s}}_{\text{VN}}\left(G\right)+\\ \frac{\kappa }{2}\left[\ln \frac{\mathrm{\Pi }{d}_i}{\mathrm{\Sigma }{d}_i}+\frac{1}{3}{\displaystyle \sum }_{\left(i,k\right)\in E}{\widehat{l}}_{ik}\left({\displaystyle \sum }_j{\widehat{l}}_{ij}{\widehat{l}}_{jk}\right)+\frac{11}{6}\right]+\\ \frac{n-1}{2}\left[\ln \left|{\mathrm{\Omega }}_{\text{ij}}\right|-\frac{7\kappa \hspace{0.17em}\ln \hspace{0.17em}2}{6}-\ln {\left(2\pi e\right)}^{\kappa }\right].\end{array}$

(42)

Here, we used the shorter notation ${\widehat{s}}_{ST}\left(G,r\right)\doteq s_{ST}\left(\widehat{\tau }\left(G,r\right)\right)$ . Since r is odd, after determining the lower bounds of the entropy $\mathcal{L}\mathcal{B}\left(\mathcal{U}\right)$ from $\widehat{\tau }\left(G,3\right)$ for all actions, in order to provide performance guarantees for t-bsp, we have to find an upper entropy bound for only the selected action $\widehat{\mathcal{U}}={\mathrm{argmax}}_{\mathcal{U}}{\widehat{s}}_{VN}\left(\mathcal{U}\right)$ , for example, by finding or bounding the Taylor remainder term $\epsilon \left(\widehat{\mathcal{U}},3\right)$ according to equation (38). As we can see, the relation (42) enables finding ${\widehat{s}}_{ST}\left(G,3\right)$ without doing expensive matrix factorizations which means that performance guarantees for t-bsp can be established in a very efficient way.

Equation (42) reveals also how to improve normalization of ${\widehat{s}}_{VN}\left(G\right)$ for different candidate path lengths n which corresponds to its scaled third term, that is,

${\hat{s}}_{VN}^{ \prime }\left(G\right)={\hat{s}}_{VN}\left(G\right)+\frac{\frac{n-1}{2}\left[\ln \frac{\left|{\mathrm{\Omega }}_{vij}\right|}{{\left(2\pi e\right)}^{\kappa }}-\frac{7\kappa \hspace{0.17em}\ln \hspace{0.17em}2}{6}\right]}{\left(\frac{7\kappa }{6}\right)}.$

(43)

In that way, an action ranking of ${\hat{s}}_{VN}^{ \prime }$ follows the action ranking of ${\widehat{s}}_{\text{ST}}\left(G,3\right)$ given that the node degree distribution corresponding to the second term in equation (42) does not change much and is already captured by ${\widehat{s}}_{VN}\left(G\right)$ .

5.3.1. Keeping action consistency

In general, given two different candidate actions ${\mathcal{U}}_1$ and ${\mathcal{U}}_2$ , when can we say that ordering them by one topological signature is the same as ordering by another, that is, $s_{ST}\left({\mathcal{U}}_1\right)<s_{ST}\left({\mathcal{U}}_2\right)\iff {\widehat{s}}_{VN}\left({\mathcal{U}}_1\right)<{\widehat{s}}_{VN}\left({\mathcal{U}}_2\right)$ ? For any r and $G=G\left(\mathcal{U}\right)$

${\widehat{s}}_{\text{ST}}\left(G,r\right)={\alpha }_r{\widehat{s}}_{\text{VN}}\left(G\right)+{\beta }_r\left(D\left(G\right)\right)+{\gamma }_r\left(n\right),$

where α _r , β _r , and γ _r are some functions that depend on the polynomial order r used for approximating τ(G) that follow from equation (40). For a given r, α _r is a constant, β _r is a function of graph’s node degrees, and γ _r an affine function of the number of its nodes.

The difference of topological signatures ${\widehat{s}}_{\text{ST}}$ of order r between graphs G₁ and G₂ with n₁ = |V₁| and n₂ = |V₂| can be written

$\begin{array}{l}\mathrm{\Delta }{\widehat{s}}_{\text{ST}}\left(G_1,G_2,r\right)={\widehat{s}}_{\text{ST}}\left(G_1,r\right)-{\widehat{s}}_{\text{ST}}\left(G_2,r\right)\\ ={\alpha }_r\mathrm{\Delta }{\widehat{s}}_{\text{VN}}\left(G_1,G_2\right)+\mathrm{\Delta }{\beta }_r\left(D\left(G_1\right),D\left(G_2\right)\right)+\mathrm{\Delta }{\gamma }_r\left(n_1,n_2\right).\end{array}$

(44)

Then, for any two graphs G₁ and G₂ from the control actions set, $\mathrm{\Delta }{\widehat{s}}_{\text{ST}}\left(G_1,G_2,r\right)<0$ if and only if

$\mathrm{\Delta }{\widehat{s}}_{\text{VN}}\left(G_1,G_2\right)<-\frac{\mathrm{\Delta }{\beta }_r\left(D\left(G_1\right),D\left(G_2\right)\right)+\mathrm{\Delta }{\gamma }_r\left(n_1,n_2\right)}{{\alpha }_r}.$

(45)

This is another way of formulating the relation between topological signatures in terms of action consistency between them. If the normalization is applied to ${\widehat{s}}_{\text{VN}}$ using (43), then Δγ _r (n₁, n₂) = 0.

6. Results

We evaluated our approach in Gazebo simulations of a single-robot active pose SLAM and in optimization of synthetic pose graphs. t-bsp was compared to the standard BSP with the ML inference problem solved using GTSAM library (Dellaert 2012). We studied empirically different topological signatures and their correspondences to information-theoretic cost (6) together with a time required for decision-making.

In active pose SLAM simulations, the robot performs LIDAR-based 2D SLAM assuming perfect data association. A probabilistic roadmap (PRM) (Kavraki et al. 1996) is used to discretize the environment and generate the roadmap while k-diverse shortest path algorithm (Voss et al. 2015) is used to generate topologically diverse candidate paths over it. The action generation process uses a ground truth map to validate actions and avoid possible re-planning stages, since they do not contribute to the goal of BSP analysis, which are a result of incremental map building. Local path corrections due to localization errors or environment changes are handled by the low-level controller and obstacle avoidance algorithm. Of course, in the deployment of active SLAM, an inferred map will be used in generation of candidate paths using the same principles.

Topological properties of candidate actions are determined by the configuration of obstacles in the environment and by the parameters of the PRM algorithm (e.g., number of samples) and k-diverse shortest path algorithm (e.g., k, path length, paths’ distance) which control the number and diversity of candidate actions with respect to their geometrical properties and homology class. Topological graphs corresponding to these candidate actions depend on the robot’s trajectory and sensor’s characteristics and pre-processing (SLAM front-end). In particular, we assume that a link between certain trajectory samples will exist whenever the Euclidean distance between them is below a threshold δ and relative orientation below ϑ, which model the range and the field of view with high reliability of ICP method used for matching LIDAR scans. In all simulations when predicting future measurements, we assume that the robot can follow the candidate path perfectly and that trajectory pose samples with LIDAR scans are taken after each robot’s relative displacement of 1 m and at the desired predefined goal poses.

6.1. Small-scale realistic simulation

First we validated our theoretical results in a small-scale scenario with the main goal to demonstrate properties of t-bsp in elementary planning modes (exploration and exploitation) under different probabilistic models of relative pose measurements.

The robot was performing two planning sessions (S1 and S2) with both exploration (in S1) and exploitation trajectories (in S2) considered to show influence of different candidate path topology and geometry in t-bsp. Figure 8 shows the considered scenario in Gazebo and the generated candidate paths for the robot in two planning sessions S1 (Figure 8a) and S2 (Figure 8b). To demonstrate the effect of noise level on t-bsp, we simulated three cases. All relative pose measurements had standard deviation of orientation error σ _θ either 0.01, 0.035, or 0.085 rad while we kept the same standard deviation of the position error σ _p = 0.1 m. This corresponds to three different values of ξ ∈ {0.1, 0.35, 0.85} for each planning session. The parameter δ was set to 2 m and ϑ to 2π radians, that is, without the constraint on relative orientation in topology prediction model.

Figure 8.

Gazebo scenario with robot’s start pose marked with and goal pose with in each planning session.

During planning session S1, the robot is performing mainly exploration as the environment was completely unknown in the beginning and its first goal is set far in the unknown area so its future posterior beliefs have topologies that resemble tree graphs, with loop closing edges between poses nearby in time. In planning session S2, the robot was instructed to return to the previously mapped area causing larger diversity among candidate actions. Topologies of the most and least complex graph of each planning session are shown in Figure 9 together with their corresponding posterior belief. In both planning sessions t-bsp based on all proposed signatures was action consistent, that is, correctly identified the best action which can be seen from Figure 11g-11l. A maximum of a topological signature is indeed a minimum of the joint entropy. Notice that it does not imply necessarily that the pose uncertainty will be the lowest at the goal or that the shortest trajectory will be selected as is evident form S1’s solution (Figure 9(a)-9(b)) since currently the entropy/uncertainty of the entire system is considered. This opens one other relevant research direction where uncertainty of a subset of state variables might be considered and its relation to topological information. Among exploitation trajectories, t-bsp prefers the one with larger loop closings leading to highest information gain (see Figure 9(c)-9(d)).

Figure 9.

Trajectory uncertainty after optimization and topologies of the worst/best actions calculated by topological BSP.

Our results in a realistic simulation show the promise of the proposed approach for efficiently solving BSP. All topological signatures of the posterior factor graphs are strongly correlated with the information-theoretic cost (Figure 11(g)-11(l)); yet, t-bsp based on an exact von Neumann entropy (s _VN ) and the number of spanning trees (s _ST ) of a graph outperforms standard BSP by an order of magnitude in terms of time complexity as shown in Figure 10. This is due to operation in a topological (less dimensional) space instead of a metric state space. The dimension of a graph Laplacian matrix in this problem is κ = 3 times smaller than the dimension of an information matrix because every 2D pose in the state vector corresponds to one node in the graph. The relative speed of s_VN and s _ST is similar as both of them require determining a graph spectrum of the associated Laplacian or its cofactor. On the other hand, the advantage of using ${\widehat{s}}_{\text{VN}}$ over s_VN and s _ST is that it is much faster, O(|E|), with possible incremental calculation that depends only on the node degrees which we investigate in the large-scale simulation in Subsection 6.4. It effectively enables real-time performance of t-bsp given that typically at each measurement update a limited number of factors is added to the graph. Also visible from Figure 10 is that t-bsp error bounds calculation adds a small, especially in the case with Hadamard bound, additional cost.

Figure 10.

Computation time per candidate action in each planning session (circle represents the mean, “x” the median and line interval one σ confidence region) for standard BSP, t-bsp by signatures based on the number of spanning trees (ST), the von Neumann entropy (VN exact and approx.) and for bounds determination, exact (equation (25)) and Hadamard (equation (29)). Notice that computation time of the standard BSP approach has been scaled by 10.

While t-bsp was action consistent in all sessions for all proposed topological signatures (ϵ(J, s) = 0), action ordering consistency (γ _f = 0) was kept only for s _ST in all cases, while only in the first planning session for s _VN and ${\widehat{s}}_{VN}$ . ${\gamma }_f\ne 0$ can happen when topologies among actions are very similar and then other factors, for example, noise or geometry, determine the solution. However, γ _f was still small. As we already stated, the determined bounds of the entropy can always be used to provide a globally optimal solution but at the cost of evaluating the objective function of actions inside them, that is, the ones whose lower bound is below an upper bound of the selected action. From Figures 11(a)–11(f) we can see that a negative topological signature s _ST is a very good approximation of the estimated posterior entropy even with large orientation noise, that is, large ξ values. On the other hand, its determined lower bound is very sensitive to it. For larger ξ values, bounds are less tight and therefore our performance guarantees are more conservative. In practice, however, more important is that the upper bound is close to the real entropy since we make decisions based on it. We can also notice that for higher ξ, Hadamard approximation of the lower bound is getting better, meaning that we practically have very small computational cost in determining the bounds accurately.

Figure 11.

Estimated entropy of posterior belief and its bounds determined by t-bsp (top) and correlation of t-bsp and standard BSP (bottom) for three different values of ξ in planning sessions S1 and S2. Notice different y-axis on the correlation plots since only action ordering consistency is important in BSP.

6.2. Real-world experiment

The experiment was performed on a real robot using a fixed measurement noise model in inference and planning, considering a similar scenario as in the simulation from Section 6.1. The results are presented in Extension 1 as a proof of concept of our method.

6.3. Synthetic simulation

Apart from the SLAM setting, next we consider general pose graphs randomly created and optimized by the Maximum Likelihood Estimator (MLE) and their relation to the topological signatures developed in this paper. We show that good action consistency is still kept even when comparing significantly varying pose graphs. We generated pose graphs with varying number of nodes n and edges m of which $n-1$ correspond to odometry and the rest to LC = m − n + 1 loop closing factors. We choose $LC=\mathrm{\Delta }\overline{d}n/2$ , causing average node degree of a graph increasing in steps of $\mathrm{\Delta }\overline{d}\in \left\{\mathrm{0.1,0.5,0.75,1,2}\right\}$ from a given odometry graph of size n (whose average node degree is approx. 2). For each pair (n, m) we create a group of 10 random topologies, calculate their topological signatures s _ST and ${\widehat{s}}_{VN}$ , and measure the average time for their calculation, t _ST and t _VN , respectively. The results of this simulation are given in Figure 12. We can see that correlation with the posterior entropy of FIM of MLE is still high for s _ST on the whole range of topologies while ${\widehat{s}}_{VN}$ follows the overall trend and is high for graphs with the same n, but breaks the action consistency with jumps of n (Figure 12(b), green curve). This suggests that a normalization of ${\widehat{s}}_{VN}$ should be applied according to its relation to the objective according to the Definition 1. As explained in Section 5.3, after applying correction term given by the equation (43), we can see that actions consistency improves significantly (Figure 12(b), black curve).

Figure 12.

Performance on simulated random graphs.

Although less accurate, the plot in Figure 12(a) demonstrates why ${\widehat{s}}_{VN}$ might still be interesting to use for fast decision-making in some situations. Clearly, the gain in speed is significant in high-dimensional BSP problems.

6.4. Large-scale realistic simulation

A large-scale scenario tests t-bsp performance in a complex environment depicted in Figure 13 with larger number of planning sessions. Its main goal is to investigate the proposed approach in the settings closer to active SLAM application domains. It serves also as a test bed for comparing the batch and incremental BSP algorithms.

Figure 13.

Gazebo simulation environment with Pioneer-3AT ground robot marked with white bounding box. Grid cell size is 1 m x 1 m.

For the second purpose, we design two experiments which mainly differ in the planning horizon length L considered:

(a) a goal-to-goal planning, and

(b) Model Predictive Control (MPC) style planning.

The experiment a) was primarily designed to test the execution time of the batch BSP algorithms whereas the experiment b) for their incremental counterparts. Since action consistency does not depend on the particular operation mode, it is enough to show it only once. In the first experiment, the robot is instructed to visit ten predefined goals in a given order and to plan with a horizon sufficient to connect consecutive goals. The second considers the same set of goals with the difference that re-planning is done also on several intermediate waypoints on the selected route between goals choosing the next waypoint as a sub-goal. We call this MPC mode, because the robot considers changing its immediate control action on a way. In this simulation, however, for ensuring similar conditions in both experiments, the robot does not take any detours from the original route. This mode would make sense in a dynamic environment or if the inferred map is used by the action generator method due to partial map availability and possibility that a better global path exists.

Consequentially, in goal-to-goal planning, the robot executes 10 planning sessions and in each, many measurements are added during the motion and prediction steps. Contrary to that, MPC mode has more planning sessions with fewer new measurements acquired between them. In both, the topology prediction parameter δ was set to 2 m and ϑ to 1 rad. Standard deviations of relative orientation and position measurement noise were kept constant at σ _p = 0.1 m and σ _θ = 0.01 rad.

6.4.1. Batch BSP algorithms

The standard batch BSP inference was solved by Levenberg-Marquardt algorithm from GTSAM library with the information-theoretic cost (6) calculated exactly using Cholesky factorization of the information matrix of the posterior belief required for determining the determinant. The same Cholesky method was used to factorize the reduced Laplacian matrix of the topological posterior graph needed for t-bsp based on the number of spanning trees topological signature s _ST . Also, in every planning session, the approximated von Neumann graph entropy ${\widehat{s}}_{VN}$ was calculated from scratch using the expression (13) and then normalized using (43).

As expected, the run-time of the t-bsp based on s _ST is by a constant multiplicative factor lower than the standard BSP (Figure 14) since the sparsity pattern of both information matrix and reduced Laplacian matrix is the same, whereas the latter’s dimension is κ times reduced, κ being the dimension of the robot’s pose. On the other hand, the proposed t-bsp based on ${\widehat{s}}_{VN}$ shows almost linear-time complexity on average planning time per candidate action and achieves by three orders of magnitude higher speed by the last planning session.

Figure 14.

Run-time comparison of batch BSP algorithms. Standard BSP is based on Levenberg-Marquardt optimization and explicit calculation of objective function (6) by doing Cholesky factorization of information matrix. t-bsp based on s _ST uses the same method of Cholesky factorization to calculate the number of spanning trees of a topological graph.

Importantly, an optimal action was selected in all planning sessions by the proposed t-bsp based on ${\widehat{s}}_{VN}$ as can be seen from Figure 15. Its only limitation is that optimality cannot always be guaranteed using topological entropy bounds. Information gain of candidate actions, in those planning sessions where it is the case, is indeed very similar and then non-topological factors determine the solution.

Figure 15.

Action consistency of t-bsp based on approx. von Neumann entropy ${\widehat{s}}_{VN}$ and optimality guarantees in relation to information gain of candidate actions (each dot of the same color represents one candidate action in a single planning session). Optimal action is selected by t-bsp in all planning sessions, but its optimality can be guaranteed in sessions 1, 2, 5, 7, and 10 where one action is clearly distinctive in information gain.

Let us have a closer look into two extreme cases: one in which the optimal action can be guaranteed and the other with a small percentage of suboptimal actions detected by t-bsp. The representative of the first case is the planning session 5 (S5) with relevant plots shown in Figure 16, and of the second, planning session 8 (S8) analyzed in Figure 17. The whole simulation and analysis of the rest of the planning sessions can be seen in Extension 2.

Figure 16.

Planning session 5.

Figure 17.

Planning session 8.

S5’s candidate paths together with the inferred map and the past robot’s trajectory are shown in Figures 16(a)–16(b). It can be seen that there exists a single candidate path (marked with light green color) which leads to the previously mapped area and from the information-theoretic perspective is optimal as it significantly reduces uncertainty of the robot’s trajectory as shown in Figure 16(f). For illustration, we also show the uncertainty of the worst action in that session in Figure 16(e) where ellipses represent the marginal pose’s uncertainty in both plots. In this case, topology solely is enough to disqualify other actions (Figure 16(d)).

S8’s candidate paths together with the inferred map and the past robot’s trajectory are shown in Figures 17(a)–17(b). First we notice that in that session there was a smaller number of candidate paths (only 4) which belong to two homology classes, either going left or right around the wall obstacle. Three of them are quite similar in the geometrical shape and all of them passing through already mapped areas. Therefore, it makes sense that the expected accuracy (path entropy) obtained by following either path will roughly be the same as can be seen in Figures 17(e)–17(f) for the worst and best actions from that set. Even in that unfavorable situation for t-bsp, it still managed to find an optimal action (Figure 17(c)) where maximum of ${\widehat{s}}_{VN}$ corresponds to the minimum of the joint entropy, but not to guarantee its optimality. Two other actions need to be inspected further for that, which is visible from Figure 17(d). We can use an upper topological bound which gives an uncertainty margin for deciding that. We argue that in most practical applications this difference in information gain will be insignificant and t-bsp is good for coarse-level planning especially, when a loss of precision up to the model approximation gap is higher than a difference between actions’ objective values.

6.4.2. Incremental BSP algorithms

In the experiment b) we compare running times of incremental BSP algorithms. Standard incremental BSP was realized by employing state-of-the-art incremental smoothing and mapping algorithm ISAM2 from GTSAM library. It updates Cholesky factorization of an information matrix incrementally using operations on a graphical model called Bayes tree (Kaess et al. 2010). For the purpose of this analysis, we measure the run-time of only updating the Bayes tree of posterior beliefs corresponding to the candidate actions, since after obtaining this form calculating the information-theoretic cost is a simple operation of calculating a determinant of a triangular matrix. Topological BSP is based on the approximated von Neumann entropy ${\widehat{s}}_{VN}$ signature calculated incrementally using the formula (16). The cumulative planning time across all planning sessions in MPC mode is shown in Figure 18(a) for standard BSP and in Figure 18(b) for the t-bsp based on ${\widehat{s}}_{VN}$ . Again, t-bsp outperforms standard BSP (by two orders of magnitude). For reference, we also show non-incremental t-bsp based on ${\widehat{s}}_{VN}$ which is also much faster then ISAM2 BSP. The graph in Figure 18c presents planning time per candidate action with the change of state dimension over time. As the theoretical analysis suggests, time complexity is near O(1) for the incremental t-bsp and near O(m) for its batch equivalent, where m is the number of edges in the topological graph, which in this case increases almost linearly with the state dimension.

Figure 18.

Run-time comparison of incremental BSP algorithms in MPC mode. Standard BSP is based on ISAM2 algorithm for belief propagation and information matrix factorization (Figure 18(a)). Topological BSP is based on approximated von Neumann entropy ${\widehat{s}}_{VN}$ signature (Figure 18(b)). Topological signature update per candidate action demonstrates almost constant-time complexity of incremental t-bsp and near linear-time of its batch equivalent (Figure 18(c)). The plot in Figure 18(b) shows cumulative planning time, that is, summation of planning times over the sessions and candidate actions from Figure 18(c).

7. Conclusions

This work provides theoretical foundations for information-theoretic topological belief space planning (t-bsp), a novel paradigm for highly efficient decision-making under uncertainty. The main idea consists of transforming the original optimization problem to a topological space where a solution is easier to find and then establish the error bounds with respect to the optimal solution. Doing so, we avoid belief propagation and explicitly calculating an objective function which offers significant speed-up to decision-making. Very importantly, even when using such approximations, an optimal BSP solution can still be obtained in cases when two optimization problems are action consistent, a concept introduced in this work for objective function approximations. In the general case, one can resort to t-bsp to drastically reduce computational cost while carefully monitoring a conservative estimate on the sacrifice in performance, that would be provided by the error bounds, or to find globally optimal solution by evaluating generally a much smaller number of candidate actions.

Topological signatures are defined on topologies of factor graphs that correspond to future posterior beliefs associated to candidate actions and their design depends on the objective function. In this work, we consider information-theoretic objective functions and pairwise factors whose topologies can be represented with simple graphs. Two topological signatures are proposed: normalized number of spanning trees of a graph and approximation of its von Neumann entropy by a function of node degrees. The first shows better action consistency, but the special focus in this paper is given to the second signature as it enables real-time performance of BSP while still offering good action consistency. It can be used in either batch or incremental mode. For both signatures, we derived error bounds of t-bsp that depend on topological parameters, measurement noise, and prior state estimate under maximum likelihood estimation framework and the bounds can be calculated with a small additional computational cost.

We presented an evaluation of our approach in realistic active SLAM simulations and in random pose-graph optimizations. Results show t-bsp was optimal in all planning sessions, but optimality could not always had been guaranteed depending on the measurement noise characteristics and diversity of candidate actions (topological and geometrical). However, in those sessions candidate paths carry very similar information gain and which action will be chosen is often irrelevant in practice, but when it is important, t-bsp can be used as a pre-processing tool to reduce the action search space. Also, decision-making is done by the upper entropy bound which is much more resilient to noise than the lower bound needed for the guarantees. Time performance improved drastically compared to state-of-the-art. Incremental t-bsp based on von Neumann entropy exhibits O(1) asymptotic complexity in active SLAM MPC-style planning. Also, update of the topological signature is very intuitive and its complexity is independent on which variables loop closure affects.

This work opens several research directions which might be interesting in the context of simplifications approaches for BSP. One is to investigate if a wider class of objective functions to which t-bsp is applicable exists, that is, where the solution is strongly characterized by the belief topology. Also, we might consider relaxing the assumptions on Gaussian beliefs, isotropic measurements noise or pairwise relations in measurement model used in this work. A first work in that direction is (Shienman et al. 2021) where uncertainty minimization of a predefined subset (focused set) of variables and uncorrelated differently weighted measurements are addressed, building upon the t-bsp approach for the unfocused case introduced here. Improving tightness of the error bounds would be of great interest for guarantees in fine-level planning, when control actions are topologically similar. Finally, topology prediction models could be further improved to increase prediction accuracy, for example, by taking into account probabilities of data associations. Overall, the presented approach contains both fundamental and practical contributions to belief space planning and we expect it will motivate further research and find diverse applications.

Supplemental Material

Supplemental Material