abbreviationnohyperfirsttrue rosROSRobot Operating System fovFoVField of View
gpuGPUGraphics Processing Unit cpuCPUCentral Processing Unit cudaCUDACompute
Unified Device Architecture memMEMMulti-Modal Elevation Map erfERF
NetEfficient Residual Factorized ConvNet cnnCNN Convolutional Neural Network

# MEM: Multi-Modal Elevation Mapping for Robotics and Learning

Gian Erni†, Jonas Frey†, Takahiro Miki†, Matias Mattamala‡, Marco Hutter†

###### Abstract

Elevation maps are commonly used to represent the environment of mobile robots
and are instrumental for locomotion and navigation tasks. However, pure
geometric information is insufficient for many field applications that require
appearance or semantic information, which limits their applicability to other
platforms or domains. In this work, we extend a 2.5D robot-centric elevation
mapping framework by fusing multi-modal information from multiple sources into
a popular map representation. The framework allows inputting data contained in
point clouds or images in a unified manner. To manage the different nature of
the data, we also present a set of fusion algorithms that can be selected
based on the information type and user requirements. Our system is designed to
run on the GPU, making it real-time capable for various robotic and learning
tasks. We demonstrate the capabilities of our framework by deploying it on
multiple robots with varying sensor configurations and showcasing a range of
applications that utilize multi-modal layers, including line detection, human
detection, and colorization.

Figure 1: Three different applications of our multi-modal elevation mapping
framework. Left: PCA layer of visual features on a crosswalk using LiDAR,
Depth and RGB cameras. Center: semantic segmentation layer in an agricultural
field using a RGB-D sensor. Right: semantic segmentation layer in a garden
using LiDAR, Depth and RGB cameras.  ††$\dagger$ Authors are with the
Department of Mechanical and Process Engineering, ETH Zürich, 8092 Zürich,
Switzerland {gerni, jonfrey, tamiki<EMAIL_ADDRESS>J. Frey is also
associated with the Max Planck Institute for Intelligent Systems, 72076
Tübingen, Germany. $\ddagger$ M. Mattamala is with the Oxford Robotics
Institute at the University of Oxford, UK<EMAIL_ADDRESS>This project
was supported by the Max Planck ETH Center for Learning Systems (Frey), Swiss
National Science Foundation (SNSF) through project 166232, 188596, the
National Centre of Competence in Research Robotics (NCCR Robotics), and the
European Union’s Horizon 2020 research and innovation program under grant
agreement No.780883 and ANID / Scholarship Program / DOCTORADO BECAS CHILE /
2019-72200291 (Mattamala).

##  I INTRODUCTION

Autonomous unmanned ground vehicles deployed in outdoor environments need to
understand their surroundings to operate in a safe and reliable manner.
Onboard range sensors, such as LiDARs, stereo cameras, or RADARs can provide
the robot with a spatial understanding of its surroundings. However, these
different representations have varying nature and noise, which motivates their
aggregation into a single unifying map.

Different types of maps have been developed for robotics. The simplest are 2D
maps [24], which store the occupancy information in each cell. 3D volumetric
maps are another popular representation [15, 20] that better encode the
geometry of the environment, though at a higher computational and memory cost.
2.5D maps (also called elevation or height maps) [5, 6, 18] are a compromise
between both, and well suited for ground platforms. All the aforementioned
maps only contain geometric information about the environment. To successfully
capture different environments, these representations need to be _multi-modal_
, here _multi-modal_ refers to different information content types i.e.,
encode semantic classes, friction, traversability, color, or other task-
specific information, which is an active area of research nowadays [4, 10, 14,
23]. The multi-modal information can significantly enhance the performance of
a variety of downstream tasks. For example, semantic information enables
robots to differentiate concrete from grass and mud, which allows them to plan
a path on the less cost-intensive road surface.

In this work, we aim to contribute to the development of multi-modal robotic
perception by presenting a flexible, real-time capable framework. It builds
upon our previous work [18] to allow the seamless integration of multi-modal
information such as geometry, semantics, and other data in various modalities
from different sources. It is implemented on a to accelerate the heavy
calculation of large data structures. In addition, our framework provides
customizable post-processing plugins as introduced in [18], enabling the map
to be adopted in a variety of tasks, including its use in learning approaches.
This allowed us to develop different robotics and learning applications in a
simple way, only using the single unified framework. Specifically, the
contributions are:

  * •

A flexible multi-modal elevation mapping framework that can be fed with
different representations such as point clouds and monocular images.

  * •

Fusion strategies to deal with multi-modal data, such as geometry, RGB
information, high-dimensional features, and semantic classes.

  * •

Demonstration of multiple applications for robotics and learning that exploit
the flexibility of the approach in terms of sensing and data modalities.

  * •

Open source implementation of the system for the benefit of the
community111https://github.com/leggedrobotics/elevation_mapping_cupy.

Figure 2: Overview of our multi-modal elevation map structure. The framework
takes multi-modal images (purple) and multi-modal (blue) point clouds as
input. This data is input into the elevation map by first associating the data
to the cells and then fused with different fusion algorithms into the various
layers of the map. Finally the map can be post-processed with various custom
plugins to generate new layers (e.g. traversability) or process layer for
external components (e.g. line detection).

##  II RELATED WORK

In this section, we review existing literature on map representations and
semantic maps.

###  II-A Map representations

First we review discrete representations for mapping, such as 2D occupancy
maps, 2.5D elevation maps, and 3D volumetric representations. 2D occupancy
maps are a common representation of the environment, that efficiently
represent free and occupied space on a horizontal grid [24]. They are suited
for flat terrain and are computationally efficient. However, they cannot
capture uneven surfaces, making them unsuitable for complex terrains. 3D
volumetric representations overcome this limitation and are able to represent
the 3D complexity of the environment [8], making them ideal for platforms such
as drones, which can freely move in 3D space. Common approaches include
octrees [15] or voxel maps [20]. These maps are capable of handling
overhanging obstacles, but creating volumetric maps comes at a cost of
increased memory and computation. For ground robots, such as wheeled and
legged platforms, 2.5D representations such as elevation maps [5] built from
3D sensing are a good compromise of expressiveness and efficiency, making them
a popular choice for robotic applications. These maps discretize the
horizontal plane into cells, but store a continuous height value for each
cell, allowing them to represent complex and rough terrain such as staircases,
steep slopes, or cluttered surfaces, which are particularly relevant for
ground platforms. Multiple implementations of elevation mapping systems have
been developed for systems, such as Fankhauser et al. [6] and Ewen et al. [4].
Pan et al. [21] and Miki et al. [18] introduce a GPU-based implementation that
enables real-time use of 2.5D elevation maps even with large amounts of data.
Our work builds upon the latter and extends it for RGB sensors, as well as
adding new features to store semantic information.

TABLE I: Comparison of features of our work with prior work. Feature |  | Elmap | Selmap | Multitask | Matur.
---|---|---|---|---|---
|  | [18] | [4] | [10] | [17]
Class probability | ✓ | ✗ | ✓ | ✓ | ✓
Sem. Features | ✓ | ✗ | ✗ | ✓ | ✗
Real-time | ✓ | ✓ | ✓ | (✓) | ✓
Monocular image | ✓ | ✗ | ✗ | ✗ | ✓
ROS | ✓ | ✓ | ✓ | ✓ | ✓
GPU | ✓ | ✓ | ✗ | ✗ | ?
Open source | ✓ | ✓ | ✓ | ✓ | ✗

###  II-B Semantic Mapping

To successfully perform certain tasks such as navigation and locomotion,
purely geometric information does not suffice and semantic information is
imperative [10]. Accumulation of the generated multi-task data in a map is
desirable for downstream applications. This has been done for 3D volumetric
maps [14, 23], as well as for 2.5D semantic elevation maps [4, 17]. In the
following, we will review the semantic mapping approaches applied to ground
robots and provide an overview of the existing methods and their features in
Tab. I.

In [17], a robot-centric 2.5D elevation map is generated by fusing a LiDAR
point cloud into the map. A pixelwise semantic segmentation is extracted from
images with a neural network and fused into the map using Bayesian inference.
Hence, the map mainly contains fixed class probabilities given by the network.
While our work is inspired by this fusion approach, we aim to extend the
formulation to any kind of data modality. Additionally, as shown in Tab. I,
this work is closed-source.

In [10] the authors present a multi-tasking neural network that takes RGB
images as input and outputs class probabilities and traversability
estimations. The semantics in combination with their depth maps are fused into
a multi-layer semantic 3D volumetric map with a Bayesian inference approach
[11], that leverages local correlations of the grid. This method runs on a and
it is fixed to the map frame targeting large-scale semantic reconstructions.
This volumetric approach runs with logarithmic complexity. While it is able to
extend to new information modalities, new tasks need to be added to the multi-
task network which requires retraining the entire custom network. As shown in
Tab. I, the method is not able to process monocular images containing multi-
modal information.

In [4] the authors present a method that generates a robot-centric elevation
map that takes data from a single RGB-D camera as input and fuses the
semantics into the map by associating the pixels to the elevation map with the
depth information. To update the semantic information, a Bayesian inference
technique is employed. This approach focuses on inputting class probabilities
while we aim to provide a general framework that accepts multi-modal data and
can potentially be extended for different platforms.

In this work, we extend our framework presented in [18] to additionally
capture multi-modal information. We present a highly configurable GPU-based
robot-centric elevation mapping, that can take multi-modal point clouds as
input as well as additional data modalities from multi-modal images. The
latter do not provide geometrical information but merely extend the modalities
of the map. The has already proven its effectiveness for fusing visual
traversability in concurrent work [9]. It runs in real-time and it is open
source as shown in Tab. I.

##  III APPROACH

The goal of this approach is to extend the elevation mapping presented in [18]
by additionally fusing multi-modal information into the map.

###  III-A Overview

As shown in Fig. 2, the framework can be divided into three core modules: Data
Association (Sec. III-B), Fusion Algorithms (Sec. III-C), and Post-Processing
(Sec. III-D). Additionally, before inputting the sensor data into the core
modules, the raw sensor data can be pre-processed to extract multi-modal
information, such as visual features or semantic segmentations. The framework
can take both multi-modal point clouds with additional fields as well as
multi-modal images with custom channels as input. These structures can contain
multi-modal information such as class probabilities, RGB colors, and semantic
features in each data point, which will be fused into the map. In the Data
Association module, the data is matched to the cells in the map either by
associating each data point of the point cloud to a cell or by projecting each
cell into the image. After establishing the correspondence between the data
and the cells, the Fusion Algorithm updates the values of the cells by fusing
the new information. The map consists of pure geometric as well as multi-modal
layers. The latter are task-agnostic information containers and thus do not
differentiate between different information modalities. The fusion algorithm
is layer-specific and defines which information modality can be fused into
these layers, as different modalities might need different fusion techniques.
These two modules combined generate a unified manner to input multi-modal data
into the map. As a final step, the generated layers can be used for post-
processing. The post-processing module consists of a plugin system where the
user can choose which map layers to input and whether to generate new layers,
modify the information within existing layers, or perform additional
processing for external components.

###  III-B Data association

Figure 3: Association of pixel-wise semantic information to individual cells
within the map. Each cell within the frustum of the corresponding camera is
projected onto the image plane. An efficient ray-casting approach checks if
the cell is visible or occluded by another cell based on the cell’s height.

The proposed framework establishes correspondences between map cells and the
data of a multi-modal point cloud or a monocular multi-modal image. Multi-
modal point clouds can be directly associated to cells according to the
horizontal coordinates of each point in parallel. The point cloud can be
additionally used to update the elevation information according to [18].

In contrast, multi-modal images do not provide elevation information and we,
therefore, rely on the elevation information accumulated within the map and a
ray-casting schema for the data association shown in Fig. 3. For most
applications, the number of potentially visible cells within the robot-centric
elevation map is smaller than the number of pixels contained within the image,
therefore we choose to project the cells onto the image. For each cell in the
of the camera, we evaluate if the cell is visible by checking the height of
each intermediate cell along the ray connecting the camera’s focal point and
the cell. The intermediate cells are computed using Bresenham’s algorithm [2].
A correspondence is successfully established between the cell and the image
plane if, for all intermediate cells along the ray, the map elevation is
smaller than the height of the ray connecting the camera focal point and the
cell. In other words, the cell can be seen from the camera’s perspective and
is not occluded. The respective pixel coordinate in the image plane for the
cell is computed using the pin-hole camera model with the known camera
intrinsics and extrinsics. This process is performed for each cell in
parallel.

###  III-C Fusion algorithms

Once the correspondence of the new data and the cell is defined, the semantic
data is fused into the respective layers, updating their values. To support
different types of information and also a variety of use cases we implemented
four different fusion algorithms: _Latest_ (Sec. III-C2), _Exponential
Averaging_ (Sec. III-C3), _Gaussian Bayesian Inference_ (Sec. III-C4), and
_Dirichlet Bayesian Inference_ (Sec. III-C5). The fusion algorithms can be
configured by the user for each input source separately. In addition, the user
can select which layer of the the data for each input source is fused to,
allowing to fuse data from different sources into the same layer or in
individual layers. The fusion algorithm needs to be selected based on the
information modality since certain fusion algorithms cannot be applied to
every modality. The algorithms are applied to each layer of each data point in
parallel. Given the modular design, new fusion algorithms can be added easily
according to the user’s needs. Class probabilities can be input into the
semantic mapping framework by either selecting a subset of all the classes or
by inputting the top k classes. The latter is convenient for memory
optimization if there are many different classes.

####  III-C1 Notation

Vectors are written in bold and lowercase letters, and the index $i$ is used
for measurement data points such as point cloud points and pixels. The
subscript $j$ is used to index elevation map cell elements and $t$ is the time
step of an update. Each point cloud consists of $N$ data points and for cell
$c_{j}$ there are $N_{j}$ data points that correspond to that cell. The class
information can be represented with class likelihood
$\mathbf{p}\in\mathbb{R}^{K}$ where $p_{k}\in[0,1]$ where $k$ is the class
index and $\sum_{k}^{K}p_{k}=1$ and $K$ is the total amount of classes, or in
one-hot encoded vectors $\mathbf{p}\in\mathbb{R}^{K}$ where
$p_{k}\in\\{0,1\\}$ and $\sum_{k}^{K}p_{k}=1$. $\theta_{j}$ is the value of a
layer of cell $j$. Features are vectors associated with the image content at a
certain location of a pixel or of a 3D point. The extracted features are
encoded in $d$-dimensional vectors $\mathbf{f}\in\mathbb{R}^{d}$.

####  III-C2 Latest

A straightforward method to fuse the information is to only retain the latest
measurement per cell. For the case of point clouds, it might be that multiple
data points fall into one cell. In that case the information is fused into the
semantic elevation map by layer-wise averaging all the data points that fall
within the same cell $c_{j}$ according to:

$\mathbf{a_{t,j}}=\frac{1}{N_{j}}\sum_{i=0}^{N}\mathbf{m_{i}}\cdot\mathds{1}_{\\{\mathbf{m_{i}}\in
c_{j}\\}}$ (1)

where $\mathbf{m_{i}}$ is the $i^{th}$ observation.

This fusion method can be used for all different information modalities.
However, it does not consider any past measurements making it more prone to
noise.

####  III-C3 Exponential averaging

Exponential averaging consists of assigning greater weight to more recent data
and exponentially decreasing the significance of previous data as shown in
Fig. 4b. This method improves the robustness towards noise by considering
previous measurements. For point clouds, the algorithm first computes the
average of all the measurements that correspond to a cell $c_{j}$ according to
Eq. (1). Then the new cell value is computed as:

$\boldsymbol{\theta_{t,j}}=w\cdot\mathbf{a_{t,j}}+(1-w)\cdot\boldsymbol{\theta_{t-1,j}}$
(2)

where $w$ is a user-defined weight. This fusion technique can be used for all
semantic information types as it can be accommodated to work with real numbers
$\mathbb{R}$ as well as probabilities $\mathbb{R}\in[0,1]$. Since exponential
averaging does not follow a probabilistic approach, the information stored in
the layer cannot be regarded as a probability measure. This motivates the two
following probabilistic fusion algorithms.

####  III-C4 Bayesian inference of Gaussian distributions

This method is suited for $d$-dimensional data
$\boldsymbol{f}\in\mathbb{R}^{d}$ and uses Bayesian inference with the
assumption that the data $\boldsymbol{f_{i}}$ is drawn from a Gaussian
distribution with known variance. The likelihood function is the probability
of the observations given the mean.

$p(\mathcal{F}|\boldsymbol{\mu_{f,j}})=\mathcal{N}(\mathcal{F}|\boldsymbol{\mu_{f,j}},\boldsymbol{\sigma^{2}_{f,j}}\mathit{I})$
(3)

where $\boldsymbol{\mu_{f,j}},\boldsymbol{\sigma^{2}_{f,j}}$ are the parameter
vectors describing the cell’s mean and variance and
$\mathcal{F}=\\{\boldsymbol{f_{0}},...,\boldsymbol{f_{N_{j}}}\\}$ is the set
of $N_{j}$ observations. We choose the prior to be Gaussian distributed:

$p(\boldsymbol{\mu_{f,j}})=\mathcal{N}(\boldsymbol{\mu_{f,j}}|\boldsymbol{\mu_{0,j}},\boldsymbol{\sigma^{2}_{0,j}}\mathbf{I})$
(4)

where $\boldsymbol{\mu_{0,j}},\boldsymbol{\sigma^{2}_{0,j}}$ are the parameter
vectors describing the prior’s mean and variance for cell $j$. This prior is a
conjugate prior with respect to the likelihood, thus the posterior will also
be a Gaussian distribution, and allows for closed form solution for the
posterior using Bayes rule [1]:

$p(\boldsymbol{\mu_{f,j}}|\mathcal{F})=\mathcal{N}(\boldsymbol{\mu_{f,j}}|\boldsymbol{\mu_{N,j}},\boldsymbol{\sigma^{2}_{N,j}}\mathbf{I})$
(5)

where

$\mu_{N,d}=\frac{\sigma_{f,d}^{2}}{N\sigma_{0,d}^{2}+\sigma_{f,d}^{2}}\mu_{0,d}+\frac{N\sigma_{0,d}^{2}}{N\sigma_{0,d}^{2}+\sigma_{f,d}^{2}}\mu_{ML,d}$
(6)
$\sigma_{N,d}=\frac{\sigma_{f,d}^{2}\sigma_{0,d}^{2}}{N\sigma_{0,d}^{2}+\sigma_{f,d}^{2}}$
(7)

where
$\mu_{ML,d}=\frac{1}{N_{j}}\sum_{i=1}^{N}f_{i,d}\cdot\mathds{1}_{\\{\mathbf{f_{i}}\in
c_{j}\\}}$.
In equations Eq. (6) and Eq. (7), we neglect the cell’s index $j$ for
readability purposes and $d$ indicates the index of the dimension. The closed-
form solution allows for real-time updating of the cell values. This method,
as previously mentioned, is used for real-valued data $\mathbb{R}^{d}$ and
thus it is suited for e.g. features.

####  III-C5 Bayesian inference of Dirichlet distributions

This fusion algorithm is suited for class probabilities. Cell-wise class
probabilities are represented by a categorical distribution
$\boldsymbol{\theta_{j}}\in[0,1]^{K}$, where each element represents the
probability of cell $j$ belonging to class $k$ from a fixed set of $K$ classes
$\mathcal{K}=\\{1,2,...,K\\}$.

The likelihood of the map cell is described with a categorical distribution
$p(\mathbf{m_{i}}|\boldsymbol{\theta_{j}})=\prod_{k=1}^{K}{\theta_{j,k}}^{m_{i,k}}$,
where $\mathbf{m_{i}}=[m_{i,1},...,m_{i,K}]$ is the semantic measurement. Thus
the likelihood over a set of $\mathcal{D}$ observations is:

$p(\mathcal{D}|\boldsymbol{\theta_{j}})=\prod_{k=1}^{K}{\theta_{j,k}}^{m_{k}}$
(8)

where $m_{k}=\sum^{N_{j}}_{i}m_{i,k}$ and the value $N_{j}$ is the number of
measurements that fall within the cell $j$.
Our goal is to seek the posterior over $\boldsymbol{\theta_{j}}$. Similar to
Sec. III-C4 we choose a conjugate prior, in this case, the Dirichlet
distribution:

$p(\boldsymbol{\theta_{j}}|\boldsymbol{\alpha}_{j})=\frac{\Gamma(\sum_{k=1}^{K}\alpha_{j,k})}{\prod_{k=1}^{K}\Gamma(\alpha_{j,k})}\prod_{k=1}^{K}\theta_{j,k}^{\alpha_{j,k}-1}\propto\prod_{k=1}^{K}\theta_{j,k}^{\alpha_{j,k}-1}$
(9)

where $\Gamma=\int_{0}^{\infty}t^{\alpha_{j,k}-1}e^{-t}dt$ and
$\boldsymbol{\alpha_{j}}$ are the parameters of the distribution. The
resulting posterior is a categorical distribution:

$p(\boldsymbol{\theta_{j}}|\mathcal{D},\boldsymbol{\alpha}_{j})\propto
p(\mathcal{D}|\boldsymbol{\theta_{j}})\cdot
p(\boldsymbol{\theta_{j}}|\boldsymbol{\alpha}_{j})\propto\prod_{k=1}^{K}\theta_{j,k}^{\alpha_{j,k}+m_{k}-1},$
(10)

with the closed-form solution being:

$p(\boldsymbol{\theta}_{j}|\mathcal{D},\boldsymbol{\alpha}_{j})=\frac{1}{\sum_{k=1}^{K}\alpha_{j,k}}\boldsymbol{\alpha}_{j}$
(11)
$\boldsymbol{\alpha}_{j,t}=\boldsymbol{\alpha}_{j,t-1}+\sum^{N_{j}}_{i}\mathbf{m_{i}},$
(12)

following the derivation by Bishop et al. [1]. The posterior is the updated
class probability for each cell. This Bayesian inference approach can be used
for $K$ different classes and keeps track of all past observations as shown in
Fig. 4.

Figure 4: Fusion algorithms behaviour over time. In Figure (a) the prior at
$t_{0}$ and the measurement that falls into one cell is shown. (b) depicts the
exponential decay of Class 0 confidence as a result of exponential averaging
fusion. (c) illustrates the Bayesian inference fusion method, which exhibits a
more gradual decrease in the Class 0 confidence.

###  III-D Post processing

Figure 5: Post-processing. This figure shows how the map can be employed to
generate insight and a new layer. On the left, the input image and the feature
extraction step are displayed. This data is fed into the elevation map in the
middle of the figure. The top right side displays a post-processing plugin
that generates the the left (red) and right (blue) tree lines predictions of a
vineyard. On the lower right side a plugin takes feature layers as input and
generates a PCA layer.

Once all the sensor information is contained in the map, the framework allows
to post-process the map layers to generate task-relevant data. These updates
are triggered by an event and thus run at a user-defined frequency. The
different post-processing options are implemented as plugins that can be
configured by the user with a few lines of code, as described in [18].

We enhance this system by also providing additional layers such as RGB or
semantics to the plugins. These plugins can utilize the layer information that
is already on , thus reducing the data transferring time. We demonstrate the
usability with several examples such as neural networks that take map layers
as input and generate a new layer (Fig. 5b), modify existing layers within the
, or perform additional processing for external components (Fig. 5a). Please
refer to Sec. IV for a detailed description.

##  IV EXPERIMENTS

We first show the implementation details of the framework (Sec. IV-A). We then
present the results by profiling the time performance of the implementation
(Sec. IV-B). Finally, we showcase how our framework can benefit a variety of
robotic applications with multiple case studies performed on different robots
(Sec. IV-C, Sec. IV-D, Sec. IV-E).

###  IV-A Implementation

The map is implemented in Python, using CuPy [19] which allows acceleration
and the generation of user-defined low-level kernels for customized highly
parallel data processing. It uses ROS [22] for inter-process communication and
the core modules implemented in Python are wrapped by a C++ framework to
accelerate the serialization of ROS data. The map is published at a user-
defined rate as a GridMap message [7].

###  IV-B Performance analysis

For the performance analysis, we run the elevation mapping framework with a
$$10\text{\,}\mathrm{m}$\times$10\text{\,}\mathrm{m}$$ grid with a resolution
of $4\text{\,}\mathrm{c}\mathrm{m}$ resulting in a grid of $250\times 250$
cells. The map is tested on a robot with one Stereolabs ZED 2i camera,
publishing an image of $360\times 640$ pixels at a framerate of
$3\text{\,}\mathrm{H}\mathrm{z}$. The image stream is processed with a pre-
trained semantic segmentation network [16]. All the time profiling tests were
performed over 300 iterations.

TABLE II: Table containing the time performance of the semantic elevation map processing a point cloud of 230’400 points.  Description | RTX 4090 [$\text{\,}\mathrm{m}\mathrm{s}$] | Jetson Orin [$\text{\,}\mathrm{m}\mathrm{s}$]
---|---|---
point trsf. & z err. cnt | $0.047\pm 0.013$ | $0.695\pm 0.246$
drift compensation | $0.108\pm 0.093$ | $0.915\pm 0.551$
height update & ray cast | $1.341\pm 0.107$ | $17.524\pm 2.417$
_multi-modal update_ | $0.306\pm 0.115$ | $1.536\pm 0.508$
overlap clearance | $0.226\pm 0.100$ | $0.970\pm 0.186$
traversability | $0.538\pm 0.159$ | $1.936\pm 0.200$
normal calculation | $0.030\pm 0.030$ | $0.060\pm 0.112$
Total | $2.596\pm 0.260$ | $23.635\pm 2.754$

We measure the time performance of the update as shown in Tab. II, following
[18]. The measurements are performed on a workstation with a GeForce RTX 4090
and Nvidia Jetson Orin. The total time required for the entire update is
$2.6\text{\,}\mathrm{m}\mathrm{s}$ / $23.6\text{\,}\mathrm{m}\mathrm{s}$
corresponding to an update rate of $385.2\text{\,}\mathrm{H}\mathrm{z}$ /
$42.3\text{\,}\mathrm{H}\mathrm{z}$ on the respective hardware. Fusing the
points into the elevation map (height update & ray cast) is the step that
takes the most time as it needs to fuse a high number of points into the map.

Comparing the time performance with [4], we can notice that even when having
four times more cells, we run at approximately the same speed. By adding
multi-modal data to the elevation map [18], the time performance of the map
stays in the same order of magnitude, making it real-time capable.

TABLE III: Point cloud sizes of different sensors.  Sensor | Size | Frequency
---|---|---
ZED 2i | 230’400 | $3\text{\,}\mathrm{H}\mathrm{z}$
Velodyne VLP-16 | 300’000 | $1\text{\,}\mathrm{H}\mathrm{z}$
Depth cameras | 407’040 | $15\text{\,}\mathrm{H}\mathrm{z}$
Figure 6: Time performance of the semantic fusion step with increasing numbers
of layers recorded on a Jetson Orin.

We test the scalability of the approach by running the framework on a Jetson
Orin with the previously presented configuration with an increasing number of
multi-modal layers. In Fig. 6 we show that the computational time of multi-
modal update increases linearly to the number of multi-modal layers. The total
processing time of the Jetson Orin for 20 layers is
$28.9\text{\,}\mathrm{m}\mathrm{s}$ for exponential averaging and
$29.0\text{\,}\mathrm{m}\mathrm{s}$ for Bayesian inference.

Finally, we inspect the time performance of the point cloud creation, which
consists of computing the 3D coordinates of the points starting from the RGB-D
setup presented in Sec. IV-B which publishes images at a rate of
$3\text{\,}\mathrm{H}\mathrm{z}$, processing the images, and publishing the
point cloud. The bottleneck in this configuration proves to be running the
image through a Lite R-ASPP Network with a MobileNetV3-Large backbone [16]
that takes $31\text{\,}\mathrm{m}\mathrm{s}$. However, given that our
framework does not depend on the specific network the user can select the
network according to the time and performance requirements.

Our approach is memory efficient. The point cloud with $100^{\prime}000$
points with x, y, z, RGB, and 5 semantic channels solely needs
$7.2\text{\,}\mathrm{M}\mathrm{B}$ of memory. The 2.5D with $200\times 200$
cells and the same amount of semantic channels requires
$3.8\text{\,}\mathrm{M}\mathrm{B}$ of memory. Hence, we only require
$1.6\text{\,}\mathrm{M}\mathrm{B}$ of additional memory space in comparison to
the elevation map of [18] which allocates $2.2\text{\,}\mathrm{M}\mathrm{B}$.
Additionally, as described in III-C5, the memory consumption can be further
reduced by choosing the solution where we capture the top k class
probabilities.

###  IV-C Colorization from multi-modal data

Figure 7: Color layer: The semantic elevation map is used in an outdoor
environment with three point clouds and three images as input and displays the
color layer.

Our framework is able to fuse color information from different data
modalities, whereas previous approaches always required a colorized point
cloud or a depth-aligned RGB image. This is particularly useful for robots
with a diverse set of sensors. In Fig. 7, a qualitative analysis shows that
the map can accurately represent the colors of the environment. The sensing
setup consists of a Sevensense Alphasense Core camera unit with three
monocular color cameras with global shutter, that capture images at a frame
rate of $9\text{\,}\mathrm{H}\mathrm{z}$ with a resolution of $1080\times
1440$ pixels and four Intel RealSense D435 depth cameras available on the
ANYbotics ANYmal C legged robot, capturing images at a frame rate of
$15\text{\,}\mathrm{H}\mathrm{z}$ with a resolution of $480\times 848$ pixels.
The layer can be used to improve the interpretability of the map by humans, as
well as for downstream learning tasks.

###  IV-D 2.5D Semantic Segmentation

Figure 8: Semantic segmentation mask layer of the elevation map. The colors of
the elevation map encode the highest class probabilities. The human lying in
the high grass is not distinguishable using the height (geometric) information
but the semantics clearly detect the person (orange).

In this second application, we demonstrate that the framework can be used to
project class probabilities into the map. In this specific case study, we use
a similar setup as described in Sec. IV-B where a stereo camera with a
resolution of $360\times 640$ pixels is used. The image is processed with
Detectron2 [13] which outputs pixel-wise class probabilities. The class
probabilities and the depth map are used to generate a semantic point cloud.
These points are then fused into the according to the Bayesian inference of
Dirichlet distributions described in Sec. III-C5. We showcase the pipeline in
an agricultural setting as shown in Fig. 8. The figure depicts a person lying
in the grass, who is identified by the segmentation network. A purely
geometric perception of the environment is not sufficient to recognize the
human in the high grass. By integrating the semantics into the elevation map
it can be used by a local planning module, which can take into account
geometric as well as semantic obstacles. This example showcases the importance
of multi-modal and more specifically semantic information for real-world
robotic applications.

###  IV-E Line detection in agricultural setting

We show that the proposed framework can be extended to take features as input,
fuse them into the map, and train a custom network that takes multiple layers
as input. Specifically, we showcase visual features representing latent
embeddings of RGB images in an agricultural setting where we predict the tree
lines in special crops as shown in Fig. 5a. The described framework is tested
on a wheeled robot with one RGB-D camera, publishing an image of $360\times
640$ pixels. In this application, we extract features from the RGB image with
a self-supervised pre-trained vision transformer (ViT) [3]. We fuse these
features according to the Bayesian inference of Gaussian distributions
described in Sec. III-C4. In this use case, we use elevation and semantic
information as input to an end-to-end learnable network that predicts two tree
lines in special crop fields. The goal of the model is to predict the line
parameters in the robot’s frame. The lines are modeled as second-order
polynomials. The machine learning pipeline consists of two sequential blocks
and follows the work of [12]. The first block is a that generates weight maps
from the elevation and feature data. The second block takes the weight maps as
input and predicts the line parameters of the tree line. The consists of the .
It predicts a weight map with 2 channels, one for each predicted line. The
output of the is fed into the second block consisting of a least squares layer
(LSQ) predicting the line parameters. The predicted lines resulting from the
approach can enable robot navigation, even in the presence of state estimation
inaccuracies.

##  V CONCLUSION

In this work we extended a state-of-the-art 2.5D elevation map by including
multi-modal information of the environment, in order to provide an easy-to-use
and computationally efficient tool for various robotics and learning tasks. We
presented a highly configurable framework that allows the user to configure
the map content and enables a great variety of downstream tasks. We have shown
high performance and low memory consumption of our framework running on . To
demonstrate that our approach can be used for various use cases, we have shown
three different example applications. We showed that the RGB colors can be
injected into the map, and we demonstrated that various pre-trained semantic
segmentation networks can be used to update the layers. Finally, we introduced
a learning-based tree line detection of special crops which uses the feature
and elevation layers of the as input. We hereby demonstrate that our framework
serves as a highly configurable environmental representation tool which is
easy to use and openly accessible for the research community.

## References

  * [1] C. M. Bishop, _Pattern recognition and machine learning, 5th Edition_ , ser. Information science and statistics. Springer, 2007.
  * [2] J. Bresenham, “Algorithm for computer control of a digital plotter,” _IBM Syst. J._ , vol. 4, no. 1, pp. 25–30, 1965.
  * [3] M. Caron, H. Touvron, I. Misra, H. Jégou, J. Mairal, P. Bojanowski, and A. Joulin, “Emerging properties in self-supervised vision transformers,” in _2021 IEEE/CVF International Conference on Computer Vision, ICCV 2021, Montreal, QC, Canada, October 10-17, 2021_. IEEE, 2021, pp. 9630–9640.
  * [4] P. Ewen, A. Li, Y. Chen, S. Hong, and R. Vasudevan, “These maps are made for walking: Real-time terrain property estimation for mobile robots,” _IEEE Robotics Autom. Lett._ , vol. 7, no. 3, pp. 7083–7090, 2022.
  * [5] P. Fankhauser, M. Bloesch, C. Gehring, M. Hutter, and R. Siegwart, “Robot-centric elevation mapping with uncertainty estimates,” _Mobile Service Robotics: Proceedings of the 17th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines_ , pp. 433–440, 2014.
  * [6] P. Fankhauser, M. Bloesch, and M. Hutter, “Probabilistic terrain mapping for mobile robots with uncertain localization,” _IEEE Robotics Autom. Lett._ , vol. 3, no. 4, pp. 3019–3026, 2018.
  * [7] P. Fankhauser and M. Hutter, “A Universal Grid Map Library: Implementation and Use Case for Rough Terrain Navigation,” in _Robot Operating System (ROS) – The Complete Reference (Volume 1)_ , A. Koubaa, Ed. Springer, 2016, ch. 5.
  * [8] J. Frey, D. Hoeller, S. Khattak, and M. Hutter, “Locomotion policy guided traversability learning using volumetric representations of complex environments,” in _2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, oct 2022.
  * [9] J. Frey, M. E. Mattamala Aravena, N. Chebrolu, C. Cadena, M. Fallon, and M. Hutter, “Fast traversability estimation for wild visual navigation,” in _Proceedings of Robotics: Science and System XIX_. Robotics Science & Systems Foundation, 2023, Conference Paper, robotics: Science and Systems (RSS 2023); Conference Location: Daegu, South Korea; Conference Date: July 10-14, 2023.
  * [10] L. Gan, Y. Kim, J. W. Grizzle, J. M. Walls, A. Kim, R. M. Eustice, and M. Ghaffari, “Multitask learning for scalable and dense multilayer bayesian map inference,” _IEEE Transactions on Robotics_ , vol. 39, no. 1, pp. 699–717, 2023.
  * [11] L. Gan, R. Zhang, J. W. Grizzle, R. M. Eustice, and M. G. Jadidi, “Bayesian spatial kernel smoothing for scalable dense semantic mapping,” _IEEE Robotics Autom. Lett._ , vol. 5, no. 2, pp. 790–797, 2020.
  * [12] W. V. Gansbeke, B. D. Brabandere, D. Neven, M. Proesmans, and L. V. Gool, “End-to-end lane detection through differentiable least-squares fitting,” in _2019 IEEE/CVF International Conference on Computer Vision Workshops, ICCV Workshops 2019, Seoul, Korea (South), October 27-28, 2019_. IEEE, 2019, pp. 905–913.
  * [13] R. Girshick, I. Radosavovic, G. Gkioxari, P. Dollár, and K. He, “Detectron,” https://github.com/facebookresearch/detectron, 2018.
  * [14] M. Grinvald _et al._ , “Volumetric instance-aware semantic mapping and 3d object discovery,” _IEEE Robotics Autom. Lett._ , vol. 4, no. 3, pp. 3037–3044, 2019.
  * [15] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, “Octomap: an efficient probabilistic 3d mapping framework based on octrees,” _Auton. Robots_ , vol. 34, no. 3, pp. 189–206, 2013.
  * [16] A. Howard _et al._ , “Searching for mobilenetv3,” in _2019 IEEE/CVF International Conference on Computer Vision, ICCV 2019, Seoul, Korea (South), October 27 - November 2, 2019_. IEEE, 2019, pp. 1314–1324.
  * [17] D. Maturana _et al._ , “Real-time semantic mapping for autonomous off-road navigation,” in _Field and Service Robotics, Results of the 11th International Conference, FSR 2017, Zurich, Switzerland, 12-15 September 2017_ , ser. Springer Proceedings in Advanced Robotics, vol. 5. Springer, 2017, pp. 335–350.
  * [18] T. Miki, L. Wellhausen, R. Grandia, F. Jenelten, T. Homberger, and M. Hutter, “Elevation mapping for locomotion and navigation using GPU,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2022, Kyoto, Japan, October 23-27, 2022_. IEEE, 2022, pp. 2273–2280.
  * [19] R. Okuta, Y. Unno, D. Nishino, S. Hido, and Crissman, “Cupy : A numpy-compatible library for nvidia gpu calculations,” 2017.
  * [20] H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. I. Nieto, “Voxblox: Incremental 3d euclidean signed distance fields for on-board MAV planning,” in _2017 IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS 2017, Vancouver, BC, Canada, September 24-28, 2017_. IEEE, 2017, pp. 1366–1373.
  * [21] Y. Pan _et al._ , “Gem: Online globally consistent dense elevation mapping for unstructured terrain,” _IEEE Transactions on Instrumentation and Measurement_ , vol. 70, pp. 1–13, 2021.
  * [22] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Ng, “Ros: an open-source robot operating system,” vol. 3, 01 2009.
  * [23] A. Rosinol, M. Abate, Y. Chang, and L. Carlone, “Kimera: an open-source library for real-time metric-semantic localization and mapping,” in _2020 IEEE International Conference on Robotics and Automation, ICRA 2020, Paris, France, May 31 - August 31, 2020_. IEEE, 2020, pp. 1689–1696.
  * [24] S. Thrun and et al., “Stanley: The robot that won the DARPA grand challenge,” _J. Field Robotics_ , vol. 23, no. 9, pp. 661–692, 2006.