# Active Bayesian Multi-class Mapping from Range and Semantic Segmentation
Observations††thanks: We gratefully acknowledge support from ARL DCIST CRA
W911NF-17-2-0181 and ONR N00014-18-1-2828. The Unity simulation used for
evaluation is developed by ARL for the DCIST project: www.dcist.org.

Arash Asgharivaskasi  Nikolay Atanasov The authors are with the Department of
Electrical and Computer Engineering, University of California San Diego, CA
92093, USA<EMAIL_ADDRESS>
###### Abstract

Many robot applications call for autonomous exploration and mapping of unknown
and unstructured environments. Information-based exploration techniques, such
as Cauchy-Schwarz quadratic mutual information (CSQMI) and fast Shannon mutual
information (FSMI), have successfully achieved active binary occupancy mapping
with range measurements. However, as we envision robots performing complex
tasks specified with semantically meaningful objects, it is necessary to
capture semantic categories in the measurements, map representation, and
exploration objective. This work develops a Bayesian multi-class mapping
algorithm utilizing range-category measurements. We derive a closed-form
efficiently computable lower bound for the Shannon mutual information between
the multi-class map and the measurements. The bound allows rapid evaluation of
many potential robot trajectories for autonomous exploration and mapping. We
compare our method against frontier-based and FSMI exploration and apply it in
a 3-D photo-realistic simulation environment.

##  I Introduction

Real-time understanding, accurate modeling, and efficient storage of a robot’s
environment are key capabilities for autonomous operation. Occupancy grid
mapping [1, 2] is a simple, yet widely used and effective, technique for
distinguishing between traversable and occupied space surrounding a mobile
robot. However, as our vision of delegating increasingly sophisticated tasks
to autonomous robots expands, so should the representation power of online
mapping algorithms. Augmenting traditional geometric models with semantic
information about the context and object-level structure of the environment
has become a mainstream problem in robotics [3, 4, 5]. Robots are also
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 [6]. Our approach extends information-theoretic active
mapping techniques [7, 8, 9] from binary to multi-class environment
representations. We introduce 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 contribution is
the derivation of a closed-form efficiently computable lower bound for the
Shannon mutual information between a multi-class occupancy map and a set of
range-category measurements. This lower bound allows rapid evaluation of many
potential robot trajectories online to (re-)select one that leads to the best
trade-off between uncertainty reduction and efficient exploration of the
metric-semantic map. Unlike traditional class-agnostic exploration methods,
our model and information measure incorporate 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
motion kinematics, making it suitable for either ground or aerial robots,
equipped with either camera or Lidar sensors, exploring either indoor or
outdoor environments.

Figure 1: A robot autonomously explores an unknown environment using an RGBD
sensor and a semantic segmentation algorithm.

##  II Related Work

Frontier-based exploration [10] is a seminal work that highlights the utility
of autonomous exploration and active mapping. It inspired methods [11, 12]
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 methods continue to be widely employed in active
perception. Recent works include semantics-assisted indoor exploration [13],
active 3D coverage and reconstruction [14], and Laplace potential fields for
safe outdoor exploration [15].

Alternative techniques for active mapping use probabilistic environment models
and information-theoretic utility functions to measure and minimize the model
uncertainty. The work by Elfes [16] is among the first to propose an
information-based utility function. Information-based exploration strategies
have been devised for uncertainty minimization in robot localization or
environment mapping [17, 18, 19]. 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
[20] 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) [21, 22, 23, 24], aiming
to determine a sensing trajectory that balances robot state uncertainty and
visitation of unexplored map regions. Stachniss et al. [25] approximate
information gain for a Rao-blackwellized particle filter over the joint state
of robot pose and map occupancy. Julian et al. [7] prove that, for range
measurements, 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) [8] and fast Shannon mutual information (FSMI) [9] offer
efficiently computable closed-form objectives for active occupancy mapping
with range measurements. Henderson et al. [26] propose an even faster
computation based on a recursive expression for Shannon mutual information in
continuous maps.

This paper is most related to CSQMI [8] and FSMI [9] 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 from range-category measurements.

##  III Problem Statement

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

$\mathbf{X}_{t}:=\begin{bmatrix}\mathbf{R}_{t}&\mathbf{p}_{t}\\\
\mathbf{0}^{\top}&1\end{bmatrix},\qquad\mathbf{X}_{t+1}=\mathbf{X}_{t}\exp{(\hat{\mathbf{u}}_{t})},$
(1)

where $\mathbf{R}_{t}\in SO(3)$ is the robot orientation,
$\mathbf{p}_{t}\in\mathbb{R}^{3}$ is the robot position, 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)$. The robot is
navigating in an environment represented as 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 buildings, animals, 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 ${\cal F}:=\left\\{\boldsymbol{\eta}_{b}\right\\}_{b}$, where
$\boldsymbol{\eta}_{b}\in\mathbb{R}^{3}$ with
$\|\boldsymbol{\eta}_{b}\|_{2}=r_{max}$ and $r_{max}>0$ is the maximum sensing
range.

######  Definition 1.

A _sensor observation_ at time $t$ from robot pose $\mathbf{X}_{t}$ is a
collection ${\cal Z}_{t}:=\left\\{\mathbf{z}_{t,b}\right\\}_{b}$ of range and
category measurements $\mathbf{z}_{t,b}:=(r_{t,b},y_{t,b})\in\mathbb{R}_{\geq
0}\times{\cal K}$, acquired along the sensor rays
$\mathbf{R}_{t}\boldsymbol{\eta}_{b}$ with $\boldsymbol{\eta}_{b}\in{\cal F}$
at robot position $\mathbf{p}_{t}$.

Such information may be obtained by processing the observations of an RGBD
camera or a Lidar with a semantic segmentation algorithm [6]. 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 $m_{i}$,
$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
PDF 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 PDF is:

$p_{t+1}(\mathbf{m})\propto p({\cal
Z}_{t+1}|\mathbf{m},\mathbf{X}_{t+1})p_{t}(\mathbf{m}).$ (2)

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.

######  Problem.

Given a prior map PDF $p_{t}(\mathbf{m})$ and a finite planning horizon $T$,
find a control sequence $\mathbf{u}_{t:t+T-1}$ for the model in (1) that
maximizes the ratio of mutual information $I\left(\mathbf{m};{\cal
Z}_{t+1:t+T}\mid{\cal Z}_{1:t}\right)$ between the map $\mathbf{m}$ and future
sensor observations ${\cal Z}_{t+1:t+T}$ and the motion cost
$J(\mathbf{X}_{t:t+T-1},\mathbf{u}_{t:t+T-1})$ of the planned robot
trajectory:

$\displaystyle\max_{\mathbf{u}_{t:t+T-1}}\frac{I\left(\mathbf{m};{\cal
Z}_{t+1:t+T}\mid{\cal
Z}_{1:t}\right)}{J(\mathbf{X}_{t:t+T-1},\mathbf{u}_{t:t+T-1})}\;\;\text{subject
to}\;\;\eqref{eq:dynamic_model},\eqref{eq:bayes_rule}.$ (3)

The precise definitions of the mutual information and motion cost terms above
are:

$\displaystyle I\left(\mathbf{m};{\cal Z}_{t+1:t+T}|{\cal
Z}_{1:t}\right):=\\!\sum_{\mathbf{m}\in{\cal K}^{N}}\\!\int\cdots\int
p(\mathbf{m},{\cal Z}_{t+1:t+T}|{\cal Z}_{1:t})$
$\displaystyle\quad\times\log\frac{p(\mathbf{m},{\cal Z}_{t+1:t+T}\mid{\cal
Z}_{1:t})}{p(\mathbf{m}|{\cal Z}_{1:t})p({\cal Z}_{t+1:t+T}|{\cal
Z}_{1:t})}\prod_{\tau=1}^{T}\prod_{b}d\mathbf{z}_{t+\tau,b}$ (4)
$\displaystyle
J(\mathbf{X}_{t:t+T-1},\mathbf{u}_{t:t+T-1}):=q(\mathbf{X}_{t+T})+\sum_{\tau=0}^{T-1}c(\mathbf{X}_{t+\tau},\mathbf{u}_{t+\tau}),$

where $q(\mathbf{X}),c(\mathbf{X},\mathbf{u})>0$ 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
[27, Ch. 9] in Sec. IV and derive an efficient approximation to the mutual
information term in Sec. V. This allows us to evaluate potential robot
trajectories online and (re-)select the one that maximizes the objective in
(3), leading to efficient active multi-class mapping.

##  IV Bayesian Multi-class Mapping

This section derives the Bayesian update in (2), using a _multinomial logit
model_ to represent $p_{t}(\mathbf{m})$. To ensure that the number of
parameters in the model scales linearly with the map size $N$, we maintain a
factorized PDF over the cells:

$p_{t}(\mathbf{m})=\prod_{i=1}^{N}p_{t}(m_{i}).$ (5)

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

$\mathbf{h}_{t,i}:=\begin{bmatrix}\log\frac{p_{t}(m_{i}=0)}{p_{t}(m_{i}=0)}&\cdots&\log\frac{p_{t}(m_{i}=K)}{p_{t}(m_{i}=0)}\end{bmatrix}^{\top}\\!\\!\in\mathbb{R}^{K+1},$
(6)

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 PDF of cell $m_{i}$ may be recovered
using the softmax function $\sigma:\mathbb{R}^{K+1}\mapsto\mathbb{R}^{K+1}$:

$p_{t}(m_{i}=k)=\sigma_{k+1}(\mathbf{h}_{t,i}):=\frac{\mathbf{e}_{k+1}^{\top}\exp(\mathbf{h}_{t,i})}{\mathbf{1}^{\top}\exp(\mathbf{h}_{t,i})},$
(7)

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

######  Definition 2.

The _inverse observation model_ of a range-category measurement $\mathbf{z}$
obtained from robot pose $\mathbf{X}$ along sensor ray
$\boldsymbol{\eta}\in{\cal F}$ with respect to map cell $m_{i}$ is a
probability density function
$p(m_{i}|\mathbf{z};\mathbf{X},\boldsymbol{\eta})$.

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

######  Proposition 1.

Let $\mathbf{h}_{t,i}$ be the log odds of cell $m_{i}$ at time $t$. Given
sensor observation ${\cal Z}_{t+1}$, the posterior log-odds are:

$\mathbf{h}_{t+1,i}=\mathbf{h}_{t,i}+\sum_{\mathbf{z}\in{\cal
Z}_{t+1}}\left(\mathbf{l}_{i}(\mathbf{z})-\mathbf{h}_{0,i}\right)$ (8)

where $\mathbf{l}_{i}(\mathbf{z})$ is the inverse observation model log odds:

$\mathbf{l}_{i}(\mathbf{z}):=\begin{bmatrix}\log\frac{p(m_{i}=0|\mathbf{z})}{p(m_{i}=0|\mathbf{z})}&\cdots&\log\frac{p(m_{i}=K|\mathbf{z})}{p(m_{i}=0|\mathbf{z})}\end{bmatrix}^{\top}.$
(9)

###### Proof.

See Appendix A. ∎

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 resulting labeled range measurement $\mathbf{z}=(r,y)$
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 [28]. We parameterize the inverse observation model log-odds vector
as:

$\displaystyle\mathbf{l}_{i}((r,y)):=\begin{cases}\boldsymbol{\phi}^{+}+\mathbf{E}_{y+1}\boldsymbol{\psi}^{+},&\text{$r$
indicates $m_{i}$ is occupied},\\\ \boldsymbol{\phi}^{-},&\text{$r$ indicates
$m_{i}$ is free},\\\ \mathbf{h}_{0,i},&\text{otherwise},\end{cases}$ (10)

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 inverse observation model
$p(m_{i}=k|\mathbf{z})=\sigma_{k+1}(\mathbf{l}_{i}(\mathbf{z}))$, which is
piece-wise constant along the sensor ray.

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

$\displaystyle p(\mathbf{z}_{\tau,b}|{\cal
Z}_{1:t})=\frac{p_{t}(m_{i_{\tau,b}^{*}}=y)}{\gamma_{\tau,b}(i_{\tau,b}^{*})}\mkern-18.0mu\prod_{i\in{\cal
R}_{\tau,b}(r)\setminus\\{i_{\tau,b}^{*}\\}}\mkern-18.0mup_{t}(m_{i}=0).$ (11)

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 [7, 8, 9]. Knowing how an
observation affects the map PDF $p_{t}(\mathbf{m})$, we now switch our focus
to computing of the mutual information between a sequence of observations
${\cal Z}_{t+1:t+T}$ and the multi-class occupancy map $\mathbf{m}$.

##  V Informative Planning

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:

${\cal R}_{\tau,b}(r_{max})\cap{\cal
R}_{\tau^{\prime},b^{\prime}}(r_{max})=\emptyset.$ (12)

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. Consequently, 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 [8] but we
generalize their results to multi-class observations and map.

######  Proposition 2.

Given a sequence of labeled range observations ${\cal Z}_{t+1:t+T}$, let
$\underline{{\cal
Z}}_{t+1:t+T}=\left\\{\mathbf{z}_{\tau,b}\right\\}_{\tau=t+1,b=1}^{t+T,B}$ be
a subset of non-overlapping measurements that satisfy (12). Then, the Shannon
mutual information between ${\cal Z}_{t+1:t+T}$ and a multi-class occupancy
map $\mathbf{m}$ can be lower bounded as:

$\displaystyle I(\mathbf{m};$ $\displaystyle{\cal Z}_{t+1:t+T}|{\cal
Z}_{1:t})\geq I\left(\mathbf{m};\underline{{\cal Z}}_{t+1:t+T}|{\cal
Z}_{1:t}\right)$ (13)
$\displaystyle=\sum_{\tau=t+1}^{t+T}\sum_{b=1}^{B}\sum_{k=1}^{K}\sum_{n=1}^{N_{\tau,b}}p_{\tau,b}(n,k)C_{\tau,b}(n,k),$

where $N_{\tau,b}:=|{\cal R}_{\tau,b}(r_{max})|$,

$\displaystyle p_{\tau,b}(n,k)$
$\displaystyle:=p_{t}(m_{i_{\tau,b}^{*}}=k)\prod_{i\in\tilde{{\cal
R}}_{\tau,b}(n)\setminus\\{i_{\tau,b}^{*}\\}}p_{t}(m_{i}=0),$ $\displaystyle
C_{\tau,b}(n,k)$
$\displaystyle:=f(\boldsymbol{\phi}^{+}+\mathbf{E}_{k+1}\boldsymbol{\psi}^{+}-\mathbf{h}_{0,i_{\tau,b}^{*}},\mathbf{h}_{t,i_{\tau,b}^{*}})$
$\displaystyle+\sum_{i\in\tilde{{\cal
R}}_{\tau,b}(n)\setminus\\{i_{\tau,b}^{*}\\}}f(\boldsymbol{\phi}^{-}-\mathbf{h}_{0,i},\mathbf{h}_{t,i}),$
$\displaystyle f(\boldsymbol{\phi},\mathbf{h})$
$\displaystyle:=\log\left(\frac{\mathbf{1}^{\top}\exp(\mathbf{h})}{\mathbf{1}^{\top}\exp(\boldsymbol{\phi}+\mathbf{h})}\right)+\boldsymbol{\phi}^{\top}\sigma(\boldsymbol{\phi}+\mathbf{h}),$

and $\tilde{{\cal R}}_{\tau,b}(n)\subseteq{\cal R}_{\tau,b}(r_{max})$ is the
set of the first $n$ map cell indices along the ray
$\mathbf{R}_{\tau}\boldsymbol{\eta}_{b}$, i.e., $\tilde{{\cal
R}}_{\tau,b}(n):=\\{i\mid i\in{\cal R}_{\tau,b}(r),|{\cal
R}_{\tau,b}(r)|=n,r\leq r_{max}\\}$.

###### Proof.

See Appendix B. ∎

Prop. 2 allows evaluating the informativeness according to (3) of any
potential robot trajectory $\mathbf{X}_{t:t+T}$, $\mathbf{u}_{t:t+T-1}$. We
use a motion planning algorithm to obtain a set of trajectories to the map
frontiers, determined from the current map PDF $p_{t}(\mathbf{m})$. Alg. 1
summarizes the procedure for determining a state-control trajectory
$\mathbf{X}^{*}_{t:t+T}$, $\mathbf{u}^{*}_{t:t+T-1}$ that maximizes the
objective in (3). This kinematically feasible trajectory can be tracked by a
low-level controller that takes the robot dynamics into account. We evaluate
the proposed active multi-class mapping algorithm next.

Algorithm 1 Information-theoretic Path Planning

1:$\mathbf{X}_{t}$, $p_{t}(\mathbf{m})$

2:${\cal F}=\textsc{findFrontiers}(p_{t}(\mathbf{m}))$

3:for $f\in{\cal F}$ do

4:
$\mathbf{X}_{t+1:t+T},\mathbf{u}_{t:t+T-1}=\textsc{planPath}(\mathbf{X}_{t},p_{t}(\mathbf{m}),f)$

5: Compute (3) over $\mathbf{X}_{t:t+T},\mathbf{u}_{t:t+T-1}$ via Prop. (2)

6:return $\mathbf{X}^{*}_{t:t+T}$, $\mathbf{u}^{*}_{t:t+T-1}$ with highest
value

##  VI Experiments

We compare our active multi-class mapping algorithm against two baseline
exploration strategies: frontier-based exploration (Frontier) [10] and FSMI
[9]. We compare the methods in an active binary mapping scenario in Sec. VI-A
and active multi-class mapping scenario in Sec. VI-B. All three methods use
our range-category sensor model in (10) and our Bayesian multi-class mapping
in (8) but select informative robot trajectories $\mathbf{X}_{t:t+T}$,
$\mathbf{u}_{t:t+T-1}$ based on their own criteria. Finally, in Sec. VI-C, we
apply our method in a 3-D simulated Unity environment to demonstrate large-
scale realistic active multi-class mapping.

To identify frontiers we apply edge detection on the most likely map at time
$t$ (the mode of $p_{t}(\mathbf{m})$). Then, we cluster the edge cells by
detecting the connected components of the boundaries between explore and
unexplored space. We plan a path from the robot pose $\mathbf{X}_{t}$ to the
center of each frontier using $A^{*}$ graph search. To ensure feasibility of
the planned paths, we fit a piece-wise polynomial trajectory to each path and
provide it to a low-level controller to generate $\mathbf{u}_{t:t+T-1}$.

Figure 2: Synthetic environments used for comparisons among frontier-based
exploration [10], FSMI [9], and our approach. Different semantic categories
are represented by distinct colors.

Figure 3: Simulation results for active binary occupancy mapping (top) and
active multi-class occupancy mapping (bottom) on the environments in Fig. 2.
In both cases, the results are averaged over $5$ random experiments with
random initial robot pose.

###  VI-A 2-D Binary Exploration

We consider active binary occupancy mapping first. We compare our method
against Frontier and FSMI in synthetic 2-D environments (Fig. 2). A 2-D LiDAR
sensor is simulated with additive Gaussian noise ${\cal N}(0,0.03)$. Fig. 3
(top) compares the map entropy reduction per distance traveled among the three
methods. Our method performs similarly to FSMI in that both achieve low map
entropy by traversing significantly less distance compared to Frontier.

###  VI-B 2-D Multi-class Exploration

Next, we use the same 2-D environments in Fig. 2 but introduce range-category
measurements. Range measurements are subject to additive Gaussian noise ${\cal
N}(0,0.03)$, while category measurement have a uniform misclassification
probability of $0.2$. Fig. 3 (bottom) compares the multi-class entropy
reduction per distance travelled for all three strategies. Our method reaches
the same level of map entropy as FSMI and Frontier but traverses a noticeable
shorter distance. This can be attributed to the fact that only our method
distinguishes map cells whose occupancy probabilities are the same but their
per-class probabilities differ from each other.

###  VI-C 3-D Multi-class Exploration

Finally, we evaluate our active multi-class mapping algorithm in a photo-
realistic 3-D Unity simulation, shown in Fig. 5 (f). We use a Husky robot
equipped with an RGBD camera and run a semantic segmentation algorithm over
the RGB images. The range measurements have an additive Gaussian noise of
${\cal N}(0,0.1)$. The semantic segmentation algorithm detects the true class
with a probability of $0.95$. We implemented an octree version of our Bayesian
multi-class mapping algorithm and information computation to scale the method
to a large 3-D environment. Our implementation extends the OctoMap algorithm
[29] to multi-class log-odds mapping as described in Sec. IV. The octree map
is compressed by merging cells whose multi-class probabilities reach the same
max or min threshold. This allows compressing the majority of the free cells
into few large cells. The details of the octree implementation of the active
multi-class mapping algorithm will be presented in a follow-up paper. Fig. 5
(a)-(e) shows several iterations of the exploration process. Fig. 4 shows the
change in map entropy versus elapsed time and the classification precision for
every observed semantic class.

Figure 4: (top): Entropy versus time for 3-D simulations. The flat line
segments represent the planning phase. (bottom): Precision for observed
semantic classes.

(a) The robot begins exploration.

(b) After $2$ iterations, the robot moves north to explore a larger unknown
area.

(c) At iteration $8$, the robot continues exploration by visiting unknown
sections in the north, east, and west.

(d) The robot begins to visit previously explored areas to fill partially
observed objects at iteration $19$.

(e) Multi-class occupancy map after $20$ exploration iterations

(f) Photo-realistic Unity simulation environment

Figure 5: Time lapse of autonomous exploration and multi-class mapping in a
simulated Unity environment. The robot is equipped with an RGBD sensor and
runs semantic segmentation. Different colors represent different semantic
categories (grass, dirt, building, etc.).

##  VII Conclusion

This paper developed techniques for active multi-class mapping using range and
semantic segmentation observations. Our results enable efficient mutual
information computation over multi-class maps and make it possible to optimize
for per-class uncertainty. Our experiments show that the proposed method
performs on par with the state of the art FSMI method in binary active mapping
scenarios. However, when semantic information is considered our method
outperforms existing algorithms and leads to efficient exploration and
accurate multi-class mapping. Our future work will develop an octree extension
of the probabilistic multi-class grid map and closed-form mutual information
computation, allowing our methods to scale to very large environments.

##  Appendix A Proof of Prop. 1

Applying Bayes rule in (2) and the factorization in (5) to $p_{t}(\mathbf{m})$
for some $\mathbf{z}\in{\cal Z}_{t+1}$ leads to:

$\displaystyle{\prod_{i=1}^{N}p(m_{i}|{\cal
Z}_{1:t},\mathbf{z})=\frac{p(\mathbf{z})}{p(\mathbf{z}|{\cal
Z}_{1:t})}\prod_{i=1}^{N}\frac{p(m_{i}|\mathbf{z})}{p(m_{i})}p(m_{i}|{\cal
Z}_{1:t}).}$

(14)

The term $\frac{p(\mathbf{z})}{p(\mathbf{z}|{\cal Z}_{1:t})}$ may be
eliminated by considering the odds ratio of the cell probabilities:

$\displaystyle\prod_{i=1}^{N}$ $\displaystyle\frac{p(m_{i}=k_{i}|{\cal
Z}_{1:t},\mathbf{z})}{p(m_{i}=0|{\cal Z}_{1:t},\mathbf{z})}$ (15)
$\displaystyle=\prod_{i=1}^{N}\frac{p(m_{i}=k_{i}|\mathbf{z})}{p(m_{i}=0|\mathbf{z})}\frac{p(m_{i}=0)}{p(m_{i}=k_{i})}\frac{p(m_{i}=k_{i}|{\cal
Z}_{1:t})}{p(m_{i}=0|{\cal Z}_{1:t})}.$

Since each term in both the left- and right-hand side products only depends on
one map cell $m_{i}$, the expression holds for each individual cell. Re-
writing the expression for cell $m_{i}$ in vector form, with elements
corresponding to each possible value of $k_{i}\in{\cal K}$, and taking an
element-wise log leads to:

$\displaystyle\begin{bmatrix}\log\frac{p(m_{i}=0|{\cal
Z}_{1:t},\mathbf{z})}{p(m_{i}=0|{\cal
Z}_{1:t},\mathbf{z})}&\cdots&\log\frac{p(m_{i}=K|{\cal
Z}_{1:t},\mathbf{z})}{p(m_{i}=0|{\cal
Z}_{1:t},\mathbf{z})}\end{bmatrix}^{\top}$ (16)
$\displaystyle\qquad=(\mathbf{l}_{i}(\mathbf{z})-\mathbf{h}_{0,i})+\mathbf{h}_{t,i}$

Applying (16) recursively for each element $\mathbf{z}\in{\cal Z}_{t+1}$ leads
to the desired result in (8).∎

##  Appendix B Proof of Prop. 2

Let ${\cal R}_{t+1:t+T}(r_{max}):=\cup_{\tau,b}{\cal R}_{\tau,b}(r_{max})$ be
the set of map indices which can potentially be observed by $\underline{{\cal
Z}}_{t+1:t+T}$. Using the factorization in (5) and the fact that Shannon
entropy is additive for mutually independent random variables, the mutual
information only depends on the cells whose index belongs to ${\cal
R}_{t+1:t+T}(r_{max})$, i.e.:

$\displaystyle I(\mathbf{m}$ $\displaystyle;\underline{{\cal
Z}}_{t+1:t+T}\mid{\cal Z}_{1:t})$
$\displaystyle=\sum_{\tau=t+1}^{t+T}\sum_{b=1}^{B}\sum_{i\in{\cal
R}_{\tau,b}(r_{max})}I(m_{i};\mathbf{z}_{\tau,b}\mid{\cal Z}_{1:t}).$ (17)

This is true because the measurements $\mathbf{z}_{\tau,b}\in\underline{{\cal
Z}}_{t+1:t+T}$ are independent by construction and the terms
$I(m_{i};\underline{{\cal Z}}_{t+1:t+T}\mid{\cal Z}_{1:t})$ can be decomposed
into sums of mutual information terms between single-beam measurements
$\mathbf{z}_{\tau,b}$ and the respective observed map cells $m_{i}$. The
mutual information between a single map cell $m_{i}$ and a sensor ray
$\mathbf{z}$ is equal to:

$\displaystyle I(m_{i};\mathbf{z}\mid{\cal Z}_{1:t})=$ (18)

Using the inverse observation model in (10) and the Bayesian multi-class
update in (8), we have:

$\displaystyle\sum_{k=0}^{K}p(m_{i}=k\mid\mathbf{z},{\cal
Z}_{1:t})\log{\frac{p(m_{i}=k\mid\mathbf{z},{\cal Z}_{1:t})}{p_{t}(m_{i}=k)}}$
$\displaystyle=f(\mathbf{l}_{i}(\mathbf{z})-\mathbf{h}_{0,i},\mathbf{h}_{t,i}),$
(19)

where (10) and (8) were applied a second time to the log term above. Plugging
(19) back into the mutual information expression in (18) and returning to
(17), we have:

$\displaystyle I($ $\displaystyle\mathbf{m};\underline{{\cal
Z}}_{t+1:t+T}\mid{\cal Z}_{1:t})$ (20)
$\displaystyle=\sum_{\tau=t+1}^{t+T}\sum_{b=1}^{B}\sum_{y=1}^{K}\int_{0}^{r_{max}}\biggl{(}p(\mathbf{z}_{\tau,b}=(r,y)\mid{\cal
Z}_{1:t})$ $\displaystyle\qquad\qquad\qquad\sum_{i\in{\cal
R}_{\tau,b}(r_{max})}\mkern-18.0muf(\mathbf{l}_{i}((r,y))-\mathbf{h}_{0,i},\mathbf{h}_{t,i})\biggr{)}dr.$

For $\mathbf{z}_{\tau,b}=(r,y)$, the second term inside the integral above can
be simplified to:

$\displaystyle\tilde{C}_{\tau,b}(r,y)$
$\displaystyle:=\mkern-18.0mu\sum_{i\in{\cal
R}_{\tau,b}(r_{max})}\mkern-18.0muf(\mathbf{l}_{i}((r,y))-\mathbf{h}_{0,i},\mathbf{h}_{t,i})$
(21)
$\displaystyle\phantom{:}=f(\boldsymbol{\phi}^{+}+\mathbf{E}_{y+1}\boldsymbol{\psi}^{+}-\mathbf{h}_{0,i_{\tau,b}^{*}},\mathbf{h}_{t,i_{\tau,b}^{*}})$
$\displaystyle\qquad+\mkern-18.0mu\sum_{i\in{\cal
R}_{\tau,b}(r)\setminus\\{i_{\tau,b}^{*}\\}}\mkern-18.0muf(\boldsymbol{\phi}^{-}-\mathbf{h}_{0,i},\mathbf{h}_{t,i})$

because for map indices $i\in{\cal R}_{\tau,b}(r_{max})\setminus{\cal
R}_{\tau,b}(r)$ that are not observed by $\mathbf{z}_{\tau,b}$, we have
$\mathbf{l}_{i}((r,y))=\mathbf{h}_{0,i}$ according to (10) and
$f(\mathbf{h}_{0,i}-\mathbf{h}_{0,i},\mathbf{h}_{t,i})=0$.

Next, we apply the approximation of (11) for the first term in the integral in
(20); which leads to integration over
$\tilde{p}_{\tau,b}(r,y)\tilde{C}_{\tau,b}(r,y)$ in (20). Note that
$\tilde{p}_{\tau,b}(r,y)$ and $\tilde{C}_{\tau,b}(r,y)$ are piece-wise
constant functions since ${\cal R}_{\tau,b}(r)$ is constant with respect to
$r$ as long as the beam $\mathbf{z}$ lands in cell $m_{i^{*}}$. Hence, we can
partition the integration domain over $r$ into a union of intervals where the
beam $\mathbf{z}$ hits the same cell, i.e. ${\cal R}_{\tau,b}(r)$ remains
constant:

$\int_{0}^{r_{max}}\mkern-18.0mu\tilde{p}_{\tau,b}(r,y)\tilde{C}_{\tau,b}(r,y)\,dr=\sum_{n=1}^{N_{\tau,b}}\int_{r_{n-1}}^{r_{n}}\mkern-18.0mu\tilde{p}_{\tau,b}(r,y)\tilde{C}_{\tau,b}(r,y)\,dr,$

where $N_{\tau,b}=|{\cal R}_{\tau,b}(r_{max})|$, $r_{0}=0$, and
$r_{N}=r_{max}$. From the piece-wise constant property of
$\tilde{p}_{\tau,b}(r,y)$ and $\tilde{C}_{\tau,b}(r,y)$ over the interval
$(r_{n-1},r_{n}]$, one can easily obtain:

$\begin{split}&\int_{r_{n-1}}^{r_{n}}\tilde{p}_{\tau,b}(r,y)\tilde{C}_{\tau,b}(r,y)\,dr\\\
&=\tilde{p}_{\tau,b}(r_{n},y)\tilde{C}_{\tau,b}(r_{n},y)\gamma(n)=p_{\tau,b}(n,y)C_{\tau,b}(n,y),\end{split}$
(22)

where $p_{\tau,b}(n,y)$ and $C_{\tau,b}(n,y)$ are defined in the statement of
Prop. 2. Therefore, substituting $y$ with $k$ and plugging the integration
result into (20) yields the lower bound for the mutual information between map
$\mathbf{m}$ and observations ${\cal Z}_{t+1:t+T}$ as in (13).∎

## References

  * [1] A. Elfes, “Using occupancy grids for mobile robot perception and navigation,” _Computer_ , vol. 22, no. 6, pp. 46–57, 1989.
  * [2] S. Thrun, “Learning occupancy grid maps with forward sensor models,” _Autonomous Robots_ , vol. 15, no. 2, pp. 111–127, 2003.
  * [3] S. Song, F. Yu, A. Zeng, A. X. Chang, M. Savva, and T. Funkhouser, “Semantic scene completion from a single depth image,” in _IEEE Conference on Computer Vision and Pattern Recognition (CVPR)_ , 2017, pp. 1746–1754.
  * [4] L. Gan, R. Zhang, J. W. Grizzle, R. M. Eustice, and M. Ghaffari, “Bayesian spatial kernel smoothing for scalable dense semantic mapping,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 790–797, 2020\.
  * [5] T. Wang, V. Dhiman, and N. Atanasov, “Learning navigation costs from demonstration with semantic observations,” _Proceedings of Machine Learning Research vol_ , vol. 120, pp. 1–11, 2020.
  * [6] A. Milioto and C. Stachniss, “Bonnet: An Open-Source Training and Deployment Framework for Semantic Segmentation in Robotics using CNNs,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2019\.
  * [7] B. J. Julian, S. Karaman, and D. Rus, “On mutual information-based control of range sensing robots for mapping applications,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2013, pp. 5156–5163.
  * [8] B. Charrow, S. Liu, V. Kumar, and N. Michael, “Information-theoretic mapping using cauchy-schwarz quadratic mutual information,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2015, pp. 4791–4798.
  * [9] Z. Zhang, T. Henderson, V. Sze, and S. Karaman, “Fsmi: Fast computation of shannon mutual information for information-theoretic mapping,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2019, pp. 6912–6918.
  * [10] B. Yamauchi, “A frontier-based approach for autonomous exploration,” in _IEEE International Symposium on Computational Intelligence in Robotics and Automation_ , 1997, pp. 146–151.
  * [11] W. Burgard, M. Moors, C. Stachniss, and F. E. Schneider, “Coordinated multi-robot exploration,” _IEEE Transactions on Robotics (TRO)_ , vol. 21, no. 3, pp. 376–386, 2005.
  * [12] H. H. González-Baños and J.-C. Latombe, “Navigation strategies for exploring indoor environments,” _The International Journal of Robotics Research (IJRR)_ , vol. 21, no. 10-11, pp. 829–848, 2002.
  * [13] C. Gómez, M. Fehr, A. C. Hernández, J. Nieto, R. Barber, and R. Siegwart, “Hybrid Topological and 3D Dense Mapping through Autonomous Exploration for Large Indoor Environments,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2020.
  * [14] C. Cao, J. Zhang, M. Travers, and H. Choset, “Hierarchical coverage path planning in complex 3d environments,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2020.
  * [15] R. Maffei, M. P. Souza, M. Mantelli, D. Pittol, M. Kolberg, and V. A. M. Jorge, “Exploration of 3D terrains using potential fields with elevation-based local distortions,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2020.
  * [16] A. Elfes, “Robot navigation: Integrating perception, environmental constraints and task execution within a probabilistic framework,” in _Reasoning With Uncertainty in Robotics_ , 1995, pp. 93–130.
  * [17] S. J. Moorehead, R. Simmons, and W. L. Whittaker, “Autonomous exploration using multiple sources of information,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2001, pp. 3098–3103.
  * [18] F. Bourgault, A. A. Makarenko, S. B. Williams, B. Grocholsky, and H. F. Durrant-Whyte, “Information based adaptive robotic exploration,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2002, pp. 540–545.
  * [19] A. Visser and B. Slamet, “Balancing the information gain against the movement cost for multi-robot frontier exploration,” in _European Robotics Symposium_ , 2008, pp. 43–52.
  * [20] T. Kollar and N. Roy, “Efficient optimization of information-theoretic exploration in slam,” in _AAAI Conference on Artificial Intelligence_ , 2008, p. 1369–1375.
  * [21] H. Carrillo, I. Reid, and J. A. Castellanos, “On the comparison of uncertainty criteria for active slam,” in _IEEE International Conference on Robotics and Automation_ , 2012, pp. 2080–2087.
  * [22] L. Carlone, J. Du, M. K. Ng, B. Bona, and M. Indri, “Active SLAM and exploration with particle filters using Kullback-Leibler divergence,” _Journal of Intelligent & Robotic Systems_, vol. 75, no. 2, pp. 291–311, 2014.
  * [23] N. Atanasov, J. Le Ny, K. Daniilidis, and G. J. Pappas, “Decentralized active information acquisition: Theory and application to multi-robot SLAM,” in _IEEE International Conference on Robotics and Automation_ , 2015, pp. 4775–4782.
  * [24] J. Wang, T. Shan, and B. Englot, “Virtual maps for autonomous exploration with pose slam,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2019.
  * [25] C. Stachniss, G. Grisetti, and W. Burgard, “Information gain-based exploration using Rao-Blackwellized particle filters,” in _Robotics: Science and Systems_ , 2005, pp. 65–72.
  * [26] T. Henderson, V. Sze, and S. Karaman, “An efficient and continuous approach to information-theoretic exploration,” in _IEEE International Conference on Robotics and Automation (ICRA)_ , 2020.
  * [27] S. Thrun, W. Burgard, and D. Fox, _Probabilistic Robotics_. MIT Press Cambridge, 2005.
  * [28] J. E. Bresenham, “Algorithm for computer control of a digital plotter,” _IBM Systems Journal_ , vol. 4, no. 1, pp. 25–30, 1965.
  * [29] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, “OctoMap: an efficient probabilistic 3D mapping framework based on octrees,” _Autonomous Robots_ , vol. 34, pp. 189–206, 2013.