Semantic OcTree Mapping and Shannon Mutual Information Computation for Robot Exploration

Accurate modeling, real-time understanding, and efficient storage of a robot’s environment are key capabilities for autonomous operation. Occupancy grid mapping [1, 2] is a widely used, simple, yet effective, technique for distinguishing between traversable and occupied space surrounding a mobile robot. OctoMap [3] is an extension of occupancy grid mapping, introducing adaptive resolution to improve the memory usage and computational cost of generating 3-D maps of large environments. Delegating increasingly sophisticated tasks to autonomous robots requires augmenting traditional geometric models with semantic information about the context and object-level structure of the environment [4, 5, 6]. Robots also are increasingly expected to operate in unknown environments, with little to no prior information, in applications such as disaster response, environmental monitoring, and reconnaissance. This calls for algorithms allowing robots to autonomously explore unknown environments and construct low-uncertainty metric-semantic maps in real-time, while taking collision and visibility constraints into account.

This paper considers the active metric-semantic mapping problem, requiring a robot to explore and map an unknown environment, relying on streaming distance and object category observations, e.g., generated by semantic segmentation over RGBD images [7]. See Fig. 1 as an illustration of the sensor data available for mapping and exploration. We extend information-theoretic active mapping techniques [8, 9, 10] from binary to multi-class environment representations. Our approach introduces a Bayesian multi-class mapping procedure which maintains a probability distribution over semantic categories and updates it via a probabilistic range-category perception model. Our main contributions are:

1. 1.

   a Bayesian multi-class octree mapping approach,

2. 2.

   a closed-form efficiently-computable lower bound for the Shannon mutual information between a multi-class octree map and a set of range-category measurements,

3. 3.

   efficient C++ implementation achieving real-time high-accuracy performance onboard physical robot systems in 3-D real-world experiments¹¹1Open-source software and videos supplementing this paper are available at https://arashasgharivaskasi-bc.github.io/SSMI_webpage..

Unlike a uniform-resolution grid map, our semantic OctoMap enables efficient mutual information evaluation via run-length encoding of the range-category observations. As a result, the informativeness of many potential robot trajectories may be evaluated to (re-)select one that leads to the best trade-off between map uncertainty reduction and motion cost. Unlike traditional class-agnostic exploration methods, our model and information measure capture the uncertainty of different semantic classes, leading to faster and more accurate exploration. The proposed approach relies on general range and class measurements and general pose kinematics, making it suitable for either ground or aerial robots, equipped with either camera or LiDAR sensors, exploring either indoor or outdoor environments.

We name our method Semantic octree mapping and Shannon Mutual Information (SSMI) computation for robot exploration. This paper is an extended version of our previous conference paper [11] enabling its application in large-scale 3-D environments. Direct application of the regular-grid method in the conference version to 3-D environments faces several challenges, including memory inefficiency, because 3-D environments are predominantly made up of free or unknown space, and information computation inefficiency due to naïve summation over all voxels visited by sensor rays. We introduce (a) an octree representation for Bayesian multi-class mapping, performing octree ray-tracing and leaf node merging using semantic category distributions, (b) semantic run-length encoding (SRLE) for sensor rays which enables efficient mutual information computation between a multi-class octree and range-category measurements, and (c) a comprehensive set of experiments and comparisons with state-of-the-art techniques in 2-D and 3-D environments. We demonstrate the effectiveness of SSMI in real-world mapping and autonomous exploration experiments performed in real-time onboard different mobile robot platforms operating in indoor and outdoor environments.

Frontier-based exploration [12] is a seminal work that highlighted the utility of autonomous exploration and active mapping. It inspired methods [13, 14] that rely on geometric features, such as the boundaries between free and unknown space (frontiers) and the volume that would be revealed by new sensor observations. Due to their intuitive formulation and low computational requirements, geometry-based exploration methods continue to be widely employed in active perception. Recent works include semantics-assisted indoor exploration [15], hex-decomposition-based coverage planning [16], and Laplace potential fields for safe outdoor exploration [17]. More related to our work, receding-horizon “next-best-view” planning [18] presents an active octree occupancy mapping method which executes trajectories built from a random tree whose quality is determined by the amount of unmapped space that can be explored. Similarly, the graph-based exploration methods of [19] and [20] use local random trees in free space to sample candidate viewpoints for exploration, while a global graph maintains the connections among the frontiers in the map. Cao et al. [21, 16] introduced hierarchical active 3D coverage and reconstruction which computes a coarse coverage path at the global scale followed by a local planner that ensures collision avoidance via a high-resolution path. As shown by Corah and Michael [22], coverage-based exploration strategies can be formulated as mutual-information maximization policies in the absence of sensor noise. However, in many real-world circumstances sensor measurements are corrupted by non-trivial noise, reducing the effectiveness of geometric exploration methods that do not capture probabilistic uncertainty. For example, due to the domain shift between the training data and the test environment, utilizing a pre-trained semantic segmentation model in the mapping process requires accounting for measurement uncertainty in the exploration policy.

The work by Elfes [23] is among the first to propose an information-based utility function for measuring and minimizing map uncertainty. Information-based exploration strategies have been devised for uncertainty minimization in robot localization or environment mapping [24, 25, 26]. Information-theoretic objectives, however, require integration over the potential sensor measurements, limiting the use of direct numerical approximations to short planning horizons. Kollar and Roy [27] formulated active mapping using an extended Kalman filter and proposed a local-global optimization, leading to significant gains in efficiency for uncertainty computation and long-horizon planning. Unlike geometry-based approaches, information-theoretic exploration can be directly formulated for active simultaneous localization and mapping (SLAM) [28, 29, 30, 31], aiming to determine a sensing trajectory that balances robot state uncertainty and visitation of unexplored map regions. Stachniss et al. [32] approximate information gain for a Rao-blackwellized particle filter over the joint state of robot pose and map occupancy. Julian et al. [8] prove that, for range measurements and known robot position, the Shannon mutual information is maximized over trajectories that visit unexplored areas. However, without imposing further structure over the observation model, computing the mutual information objective requires numerical integration. The need for efficient mutual information computation becomes evident in 3-D environments. Cauchy-Schwarz quadratic mutual information (CSQMI) [9] and fast Shannon mutual information (FSMI) [10] offer efficiently computable closed-form objectives for active occupancy mapping with range measurements. Henderson et al. [33] propose an even faster computation based on a recursive expression for Shannon mutual information in continuous maps.

The recent success of machine learning methods of perception has motivated learning autonomous exploration policies. Chen et al. [34] attempt to bridge the sim2sim and sim2real gaps via graph neural networks and deep reinforcement learning. This enables decision-making over graphs containing relevant exploration information which is provided by human experts in order to predict a robot’s optimal sensing action in belief space. Lodel et al. [35] introduce a deep reinforcement learning policy which recommends next best view that maximizes information gain via defining mutual information as the training reward. Zwecher et al. [36] employ deep reinforcement learning to find an exploration policy that plans collision-free coverage paths, while another neural network provides a predicted full map given the partially observed environment. Zhang et al. [37] propose a multi-agent reinforcement learning exploration method where regions of interest, free space, and robots are represented as graph nodes, and hierarchical-hops graph neural networks (H2GNN) are used to identify key information in the environment. Related to multi-robot exploration, the authors in [38] utilize an actor-critic strategy to map an unknown environment, where Voronoi partitioning divides the exploration regions among the robots. As this paper demonstrates, incorporating semantic uncertainty in addition to geometric information in the exploration process can be beneficial. Additionally, using Shannon mutual information as an objective function may help train more generalizable exploration policies because it mitigates the need for training sensor-specific models. Hence, the techniques proposed in this paper are complementary to learning approaches and can provide robustness to measurement uncertainty and domain shift caused by sensor and operational condition variations.

Active semantic mapping has recently attracted much attention due to the proliferation of fast object detection and semantic segmentation algorithms implemented on mobile robot platforms. The authors in [39] use a two-layer architecture, where the knowledge representation layer provides a belief over the environment state to the action layer, which subsequently chooses an action to gather information or execute a task. The work in [40] presents a semantic exploration policy which takes an occluded semantic point cloud of an object, finds a match in a database to estimate the full object dimensions, and then generates candidate next observation poses to reconstruct the object. The next best view is computed via a volumetric information gain metric that computes visible entropy from a candidate pose. The semantic map used in this paper is a collection of bounding boxes around objects. Active semantic mapping has also been employed to develop sample-efficient deep learning methods. Blum et al. [41] propose an active learning method for training semantic segmentation networks where the novelty (epistemic uncertainty) of the input images is estimated as the distance from the training data in the embedding space, while a path planning method maximizes novelty of future input images along the planned trajectory, assuming novel images are spatially correlated. Georgakis et al. [42] actively train a hierarchical semantic map generation model that predicts occupancy and semantics given occluded input. The authors use an ensemble of map generation models in order to predict epistemic uncertainty of the predicted map. The uncertainty is then used to choose trajectories for actively training the model with new images that differ the most with the training data of the current model. SSMI distinguishes itself from the aforementioned works by introducing a dense Bayesian multi-class mapping with a closed-form uncertainty measure, as opposed to sampling-based uncertainty estimation. Moreover, our information-theoretic objective function directly models sensor noise specifications, unlike volumetric information gain.

This paper is most related to CSQMI [9] and FSMI [10] in that it develops a closed-form expression for mutual information. However, instead of a binary map and range-only measurements, our formulation considers a multi-class map with Bayesian updates using range-category measurements. Since the same occupancy map can be derived from many different multi-class maps, the information associated with various object classes will fail to be captured if we solely rely on occupancy information, as the case in CSQMI and FSMI. Therefore, we expect to perform exploration more efficiently by using the multi-class perception model, and consequently, expanding the notion of uncertainty to multiple classes.

Consider a robot with pose $\mathbf{X}_{t}\in SE(3)$ at time $t$ and deterministic discrete-time kinematics:

where $\mathbf{R}_{t}\in SO(3)$ is the robot orientation, $\mathbf{p}_{t}\in\mathbb{R}^{3}$ is the robot position, $\tau$ is the time step, and $\mathbf{u}_{t}:=[\mathbf{v}_{t}^{\top},\boldsymbol{\omega}_{t}^{\top}]^{\top}\in{\cal U}\subset\mathbb{R}^{6}$ is the control input, consisting of linear velocity $\mathbf{v}_{t}\in\mathbb{R}^{3}$ and angular velocity $\boldsymbol{\omega}_{t}\in\mathbb{R}^{3}$. The function $\hat{(\cdot)}:\mathbb{R}^{6}\rightarrow\mathfrak{se}(3)$ maps vectors in $\mathbb{R}^{6}$ to the Lie algebra $\mathfrak{se}(3)$. See [43, Chapter 7] for a definition of the Lie groups $SO(3)$ and $SE(3)$ and the corresponding Lie algebras $\mathfrak{so}(3)$ and $\mathfrak{se}(3)$. The robot is navigating in an environment consisting of a collection of disjoint sets ${\cal E}_{k}\subset\mathbb{R}^{3}$, each associated with a semantic category $k\in{\cal K}:=\left\{0,1,\ldots,K\right\}$. Let ${\cal E}_{0}$ denote free space, while each ${\cal E}_{k}$ for $k>0$ represents a different category, such as building, vegetation, terrain (see Fig. 1).

We assume that the robot is equipped with a sensor that provides information about the distance to and semantic categories of surrounding objects along a set of rays $\left\{\boldsymbol{\eta}_{b}\right\}_{b}$, where $b$ is the ray index, $\boldsymbol{\eta}_{b}\in\mathbb{R}^{3}$ with $\|\boldsymbol{\eta}_{b}\|_{2}=r_{max}$, and $r_{max}>0$ is the maximum sensing range.

Such information may be obtained by processing the observations of an RGBD camera or a Lidar with a semantic segmentation algorithm [7]. Fig. 1 shows an example where each pixel in the RGB image corresponds to one sensor ray $\boldsymbol{\eta}_{b}$, while its corresponding values in the semantic segmentation and the depth images encode category $y_{t,b}$ and range $r_{t,b}$, respectively. The goal is to construct a multi-class map $\mathbf{m}$ of the environment based on the labeled range measurements. We model $\mathbf{m}$ as a grid of cells $i\in{\cal I}:=\{1,\ldots,N\}$, each labeled with a category $m_{i}\in{\cal K}$. In order to model noisy sensor observations, we consider a probability density function (PDF) $p({\cal Z}_{t}\mid\mathbf{m},\mathbf{X}_{t})$. This observation model allows integrating the measurements into a probabilistic map representation using Bayesian updates. Let $p_{t}(\mathbf{m}):=p(\mathbf{m}\mid{\cal Z}_{1:t},\mathbf{X}_{1:t})$ be the probability mass function (PMF) of the map $\mathbf{m}$ given the robot trajectory $\mathbf{X}_{1:t}$ and observations ${\cal Z}_{1:t}$ up to time $t$. Given a new observation ${\cal Z}_{t+1}$ obtained from robot pose $\mathbf{X}_{t+1}$, the Bayesian update to the map PMF is:

We assume that the robot pose is known and omit the dependence of the map distribution and the observation model on it for brevity. We consider the following problem.

The definitions of the mutual information and motion cost terms in (3) are:

where the integration in (4) is over all possible values of all sensor beams over all times $\mathbf{z}_{t+\tau,b}$, and the strictly positive terms $q(\mathbf{X})$ and $c(\mathbf{X},\mathbf{u})$ model terminal and stage motion costs (e.g., distance traveled, elapsed time), respectively.

We develop a multi-class extension to the log-odds occupancy mapping algorithm [44, Ch. 9] in Sec. IV and derive an efficient approximation to the mutual information term in Sec. V. In Sec. VI, we present a multi-class extension of the OctoMap [3] algorithm, alongside a fast computation of mutual information over a semantic OctoMap using run-length encoding. This allows autonomous exploration of large 3-D environments by rapidly evaluating potential robot trajectories online and (re-)selecting the one that maximizes the objective in (3). Finally, in Sec. VII, we demonstrate the performance of our approach in simulated and real-world experiments.

This section derives the Bayesian update in (2), using a multinomial logit model to represent the map PMF $p_{t}(\mathbf{m})$ where each cell $m_{i}$ of the map stores the probability of object classes in ${\cal K}$. To ensure that the number of parameters in the model scales linearly with the map size $N$, we maintain a factorized PMF over the cells:

We represent the individual cell PMFs $p_{t}(m_{i})$ over the semantic categories ${\cal K}$ using a vector of log odds:

where the free-class likelihood $p_{t}(m_{i}=0)$ is used as a pivot. Given the log-odds vector $\mathbf{h}_{t,i}$, the PMF of cell $m_{i}$ may be recovered using the softmax function $\sigma:\mathbb{R}^{K+1}\mapsto\mathbb{R}^{K+1}$:

where $\mathbf{e}_{k}$ is the standard basis vector with $k$-th element equal to $1$ and $0$ elsewhere, $\mathbf{1}$ is the vector with all elements equal to $1$, and $\exp(\cdot)$ is applied elementwise to the vector $\mathbf{h}_{t,i}$. To derive Bayes rule for the log-odds $\mathbf{h}_{t,i}$, we need to specify an observation model for the range and categry measurements.

The Bayesian update in (2) for $\mathbf{h}_{t,i}$ can be obtained in terms of the range-category inverse observation model, evaluated at a new measurement set ${\cal Z}_{t+1}$.

To complete the Bayesian multi-class mapping algorithm suggested by (8) we need a particular inverse observation model. When a sensor measurement is generated, the sensor ray continues to travel until it hits an obstacle of category ${\cal K}\setminus\{0\}$ or reaches the maximum sensing range $r_{max}$. The labeled range measurement $\mathbf{z}=(r,y)$ obtained from position $\mathbf{p}$ with orientation $\mathbf{R}$ indicates that map cell $m_{i}$ is occupied if the measurement end point $\mathbf{p}+\frac{r}{r_{max}}\mathbf{R}\boldsymbol{\eta}$ lies in the cell. If $m_{i}$ lies along the sensor ray but does not contain the end point, it is observed as free. Finally, if $m_{i}$ is not intersected by the sensor ray, no information is provided about its occupancy. The map cells along the sensor ray can be determined by a rasterization algorithm, such as Bresenham’s line algorithm [45]. We parameterize the inverse observation model log-odds vector in (9) as:

where $\mathbf{E}_{k}:=\mathbf{e}_{k}\mathbf{e}_{k}^{\top}$ and $\boldsymbol{\psi}^{+}\!,\boldsymbol{\phi}^{-}\!,\boldsymbol{\phi}^{+}\in\mathbb{R}^{K+1}$ are parameter vectors, whose first element is $0$ to ensure that $\mathbf{l}_{i}(\mathbf{z})$ is a valid log-odds vector. This parameterization leads to an inverse observation model $p(m_{i}=k|\mathbf{z})=\sigma_{k+1}(\mathbf{l}_{i}(\mathbf{z}))$, which is piecewise constant along the sensor ray. Fig. 2 illustrates our Bayesian multi-class mapping method.

To compute the mutual information between an observation sequence ${\cal Z}_{t+1:t+T}$ and the map $\mathbf{m}$ in the next section, we will also need the PDF of a range-category measurement $\mathbf{z}_{\tau,b}\in{\cal Z}_{t+1:t+T}$ conditioned on ${\cal Z}_{1:t}$. Let ${\cal R}_{\tau,b}(r)\subset{\cal I}$ denote the set of map cell indices along the ray $\mathbf{R}_{\tau}\boldsymbol{\eta}_{b}$ from robot position $\mathbf{p}_{\tau}$ with length $r$. Let $\gamma_{\tau,b}(i)$ denote the distance traveled by the ray $\mathbf{R}_{\tau}\boldsymbol{\eta}_{b}$ within cell $m_{i}$ and $i_{\tau,b}^{*}\in{\cal R}_{\tau,b}(r)$ denote the index of the cell hit by $\mathbf{z}_{\tau,b}$. We define the PDF of $\mathbf{z}_{\tau,b}=(r,y)$ conditioned on ${\cal Z}_{1:t}$ as:

This definition states that the likelihood of $\mathbf{z}_{\tau,b}=(r,y)$ at time $t$ depends on the likelihood that the cells $m_{i}$ along the ray $\mathbf{R}_{\tau}\boldsymbol{\eta}_{b}$ of length $r$ are empty and the likelihood that the hit cell $m_{i_{\tau,b}^{*}}$ has class $y$. A similar model for binary observations has been used in [8, 9, 10].

This section described how an observation affects the map PMF $p_{t}(\mathbf{m})$. Now, we switch our focus to computing the mutual information between a sequence of observations ${\cal Z}_{t+1:t+T}$ and the multi-class occupancy map $\mathbf{m}$.

Proposition 1 allows a multi-class formulation of occupancy grid mapping, where the uncertainty of a map cell depends on the probability of each class $p_{t}(m_{i}=k)$, instead of only the binary occupancy probability $1-p_{t}(m_{i}=0)$. Moreover, the inverse observation model in (10) may contain different likelihoods for the different classes which can be used to prioritize the information gathering for specific classes. Fig. 3(a) shows an example where the estimated map of an environment with $3$ classes, free, $\textit{class}_{1}$, $\textit{class}_{2}$, contains two regions with similar occupancy probability but different semantic uncertainty. In particular, the red and green walls have the same occupancy probability of $0.9$, as shown in Fig. 3(b), but the red region more certainly belongs to $\textit{class}_{1}$ and the green region has high uncertainty between the two classes. As can be seen in Fig. 3(c), the mutual information associated with a binary occupancy map cannot distinguish between the red and green regions since they both have the same occupancy probability. In contrast, the multi-class map takes into account the semantic uncertainty among different categories, as can be seen in Fig. 3(d) where the uncertain green region has larger mutual information than the certain red region.

These observations suggest that more accurate uncertainty quantification may be achieved using a multi-class instead of a binary perception model, potentially enabling a more efficient exploration strategy. However, computing the mutual information term in (4) is challenging because it involves integration over all possible values of the observation sequence ${\cal Z}_{t+1:t+T}$. Our main result is an efficiently-computable lower bound on $I\left(\mathbf{m};{\cal Z}_{t+1:t+T}|{\cal Z}_{1:t}\right)$ for range-category observations ${\cal Z}_{t+1:t+T}$ and a multi-class occupancy map $\mathbf{m}$. The result is obtained by selecting a subset $\underline{{\cal Z}}_{t+1:t+T}=\left\{\mathbf{z}_{\tau,b}\right\}_{\tau=t+1,b=1}^{t+T,B}$ of the observations ${\cal Z}_{t+1:t+T}$ in which the sensor rays are non-overlapping. Precisely, any pair of measurements $\mathbf{z}_{\tau,b}$, $\mathbf{z}_{\tau^{\prime},b^{\prime}}\in\underline{{\cal Z}}_{t+1:t+T}$ satisfies:

In practice, constructing $\underline{{\cal Z}}_{t+1:t+T}$ requires removing intersecting rays from ${\cal Z}_{t+1:t+T}$ to ensure that the remaining observations are mutually independent. The mutual information between $\mathbf{m}$ and $\underline{{\cal Z}}_{t+1:t+T}$ can be obtained as a sum of mutual information terms between single rays $\mathbf{z}_{\tau,b}\in\underline{{\cal Z}}_{t+1:t+T}$ and map cells $m_{i}$ observed by $\mathbf{z}_{\tau,b}$. This idea is inspired by CSQMI [9] but we generalize it to multi-class observations and map.

In (13), $p_{\tau,b}(n,k)$ represents the probability that the $n$-th map cell along the ray $\mathbf{R}_{\tau}\boldsymbol{\eta}_{b}$ belongs to object category $k$ while all of the previous cells are free. The function $f(\boldsymbol{\phi},\mathbf{h})$ denotes the log-ratio of the map PMF $\sigma(\mathbf{h})$ and its posterior $\sigma(\boldsymbol{\phi}+\mathbf{h})$, averaged over object categories in ${\cal K}$ (see (26) in Appendix B for more details). As a result, $C_{\tau,b}(n,k)$ is the sum of log-ratios for the first $n$ cells along the ray $\mathbf{R}_{\tau}\boldsymbol{\eta}_{b}$ under the same event as the one $p_{\tau,b}(n,k)$ is associated with. Therefore, the lower bound $I\left(\mathbf{m};\underline{{\cal Z}}_{t+1:t+T}|{\cal Z}_{1:t}\right)$ is equivalent to the expectation of summed log-ratios $C_{\tau,b}(n,k)$ over all possible instantiations of the observations in $\underline{{\cal Z}}_{t+1:t+T}$.

Proposition 2 allows evaluating the informativeness according to (3) of any potential robot trajectory $\mathbf{X}_{t:t+T}$, $\mathbf{u}_{t:t+T-1}$. In order to perform informative planning, first, we identify the boundary between the explored and unexplored regions of the map, similar to [12]. This can be done efficiently using edge detection, for example. Then, we cluster the corresponding map cells by detecting the connected components of the boundary. Each cluster is called a frontier. A motion planning algorithm is used to obtain a set of pose trajectories to the map frontiers, determined from the current map PMF $p_{t}(\mathbf{m})$. Alg. 1 summarizes the procedure for determining a trajectory $\mathbf{X}^{*}_{t:t+T}$, $\mathbf{u}^{*}_{t:t+T-1}$ that maximizes the objective in (3), where $J(\mathbf{X}_{t:t+T-1},\mathbf{u}_{t:t+T-1})$ is the length of the corresponding path. This kinematically feasible trajectory can be tracked by a low-level controller that takes the robot dynamics into account.

Evaluation of the mutual information lower bound in Proposition 2 can be accelerated without loss in accuracy for map cells along the observation rays that contain equal PMFs. In the next section, we investigate this property of the proposed lower bound within the context of OcTree-based representations. We begin with proposing a multi-class version of the OctoMap technique, where map cells with equal multi-class probabilities can be compressed into a larger voxel. Next, a fast semantic mutual information formula is presented based on compression of range-category ray-casts over OcTree representations.

Utilizing a regular-grid discretization to represent a 3-D environment has prohibitive storage and computation requirements. Large continuous portions of many real environments are unoccupied, suggesting that adaptive discretization is significantly more efficient. OctoMap [3] is a probabilistic 3-D mapping technique that utilizes an OcTree data structure to obtain adaptive resolution, e.g., combining many small cells associated with free space into few large cells. In this section, we develop a multi-class version of OctoMap and propose an efficient multi-class mutual information computation which benefits from the OcTree structure.