ETH Library

Underwater Volumetric Occupancy
Mapping with Imaging Sonar
Master Thesis
Author(s):Marti, Dominic
Publication date:2022
Permanent link:
https://doi.org/10.3929/ethz-b-000551563
Rights / license:
In Copyright - Non-Commercial Use PermittedThis page was generated automatically upon download from the ETH Zurich Research Collection.
For more information, please consult the Terms of use.

Autonomous Systems Lab
Prof. Roland Siegwart

Master Thesis

Underwater VolumetricOccupancy Mapping with
Imaging Sonar
Autumn Term 2022

Supervised by:
Nicholas Lawrance
Victor Reijgwart
Miguel Castillon

Author:Dominic Marti

Declaration of originality
The signed declaration of originality is a component of every semester paper, Bachelor’s thesis,
Master’s thesis and any other degree paper undertaken during the course of studies, including therespective electronic versions.
Lecturers may also require a declaration of originality for other written papers compiled for theircourses.

__________________________________________________________________________
I hereby confirm that I am the sole author of the written work here enclosed and that I have compiled it
in my own words. Parts excepted are corrections of form and content by the supervisor .
Title of work (in block letters):

Authored by (in block letters):
For papers written by groups the names of all authors are required.

Name(s):First name(s):With my signature I confirm that
− I have committed none of the forms of plagiarism described in the ‘Citation etiquette’ information
sheet.
− I have documented all methods, data and processes truthfully.
− I have not manipulated any data.
− I have mentioned all persons who were significant facilitators of the work .
I am aware that the work may be screened electronically for plagiarism.
Place, date

Signature(s)For papers written by groups the names of all authors are
required. Their signatures collectively guarantee the entire
content of the written paper.

Abstract
Underwater robotics is a strongly growing field with economic interests, such as inoffshore industry, underwater archaeology and inspection tasks. One of the key contributors to more autonomy under water is the capability to build volumetric mapsof the underwater world. Particularly for navigation, volumetric occupancy maps
are one of the foci in research. The sensor imagery is often provided by forward scansonars, which are mounted directly on underwater robotic platforms. Compared tolight-based sensors, they handle turbid water significantly better and the low frequencies have less problems with attenuation than light waves. Existing approaches
to build volumetric occupancy maps struggle to be cost efficient in terms of computation time or perform poorly in unknown environments. Additionally, the recoveryof elevation information from forward scan sonars poses difficult challenges for map
makers. This work introduces a novel sensor model for forward scan sonars thattakes uncertainty in three dimensions into account. This sensor model is applied
on preprocessed imagery and provides in three dimensional probability tensors. We
show that the sensor model performs well in two and three dimensions and thatelevation recovery through uncertainty quantification is a valid approach. Finally,
the model is applied to imagery from a sonar simulator, where the integration of
the model into Voxblox is verified.

iii

Acknowledgment
I would like to thank Prof. Roland Siegwart and the Autonomous Systems Lab foroffering me the possibility to conduct my thesis there.
I am grateful for the interesting insights into underwater robotics I received from
Tethys Robotics. The link to practice puts everything in research into perspective.
Special thanks go to Nick, Victor and Miguel, who went on this journey with me
and supported me with their knowledge, time and expertise. The steep learning
curve I performed would not have been possible without you.
And finally, I would like to thank my wife Iris, my friends and family for theirendless hours of listening to my thoughts without understand a word. Your time isvery much appreciated.

v

Contents
Symbolsx

1 Introduction
1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2 State of the Art . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.2.1 Underwater Mapping . . . . . . . . . . . . . . . . . . . . . . .
1.2.2 SLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.3 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.4 Goal . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1.4.1 Thesis Structure . . . . . . . . . . . . . . . . . . . . . . . . .

1
1
3
3
4
5
5
5

2 Research
2.1 System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2 Sonar . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.1 Side-Scan Sonar . . . . . . . . . . . . . . . . . . . . . . . . .
2.2.2 Forward-Scan Sonar . . . . . . . . . . . . . . . . . . . . . . .
2.3 Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.3.1 World Representation . . . . . . . . . . . . . . . . . . . . . .
2.3.2 Surface Reconstruction . . . . . . . . . . . . . . . . . . . . . .
2.3.3 Occupancy Mapping . . . . . . . . . . . . . . . . . . . . . . .
2.4 Voxblox . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

7
7
8
8
9
10
11
12
13
14

3 Mathematical Fundamentals
3.1 Sonar Basics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.1.1 Sonar Pose . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.1.2 Coordinate Transforms . . . . . . . . . . . . . . . . . . . . . .
3.1.3 Field of View . . . . . . . . . . . . . . . . . . . . . . . . . . .
3.2 Probabilistic Occupancy Mapping . . . . . . . . . . . . . . . . . . . .
3.2.1 Probabilistic Mapping Basics . . . . . . . . . . . . . . . . . .
3.2.2 Occupancy Mapping Basics . . . . . . . . . . . . . . . . . . .

17
17
19
20
21
22
22
23

4 Image Preprocessing
4.1 Noise Rejection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2 Image Downsampling . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2.1 Average Pooling . . . . . . . . . . . . . . . . . . . . . . . . .
4.2.2 Max Pooling . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.2.3 Percentile Pooling . . . . . . . . . . . . . . . . . . . . . . . .

27
28
30
30
31
32

5 Sensor Model
5.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.1.1 Measurement Definition . . . . . . . . . . . . . . . . . . . . .
5.2 Elevation Information Recovery . . . . . . . . . . . . . . . . . . . . .

34
34
35
35

vii

5.2.1 Acoustic Shading . . . . . . . . . . . . . . . . . . . . . . . . .
5.2.2 Other Approaches . . . . . . . . . . . . . . . . . . . . . . . .
5.2.3 Probabilistic Elevation Estimation . . . . . . . . . . . . . . .
Fusion Framework . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.3.1 With Carving . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.3.2 Without Carving . . . . . . . . . . . . . . . . . . . . . . . . .
Sensor Model Validation . . . . . . . . . . . . . . . . . . . . . . . . .
5.4.1 Point Response . . . . . . . . . . . . . . . . . . . . . . . . . .
5.4.2 Two Point Response . . . . . . . . . . . . . . . . . . . . . . .
5.4.3 Folding Chair . . . . . . . . . . . . . . . . . . . . . . . . . . .
Probability Scaling . . . . . . . . . . . . . . . . . . . . . . . . . . . .

36
36
37
37
38
44
45
45
46
47
49

6 Occupancy Map
6.1 Clamping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.2 Serialization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.3 Interpolation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.4 Free Space Compensation . . . . . . . . . . . . . . . . . . . . . . . .
6.5 Update Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6.6 Occupancy Threshold . . . . . . . . . . . . . . . . . . . . . . . . . .

52
52
53
55
55
56
57

7 Integration in Voxblox
7.1 Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.1.1 Old Structure . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.1.2 New Structure . . . . . . . . . . . . . . . . . . . . . . . . . .
7.2 Testing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

59
59
59
60
62

8 Conclusion
8.1 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8.2.1 Optimization Process . . . . . . . . . . . . . . . . . . . . . .
8.2.2 Future Research . . . . . . . . . . . . . . . . . . . . . . . . .

66
66
67
67
68

Bibliography

71

A Datasheets

73

5.3

5.4

5.5

Symbols
Symbolsr, θ, ϕv
zt
MrSx∥x∥, ∠xθ , ∠xϕ∆θ, ∆ϕ, τQ(µ, σ)Qcdf (µ, σ)
µ
σ
β
∆P
T ()
SM ()
ϵ

range, azimuth and elevation angle
voxel state
general measurement at time tsonar measurement
occupancy state of world point x
measured distance and angles to the world point x
angular uncertainties and surface thickness (range uncertainty)
quadratic B-spline function
cumulated density function integrated from Q(µ, σ)
difference between true coordinate and measurement
variancecompression factor
free space compensation factor
interpolation function
sensor model
occupancy threshold

Probabilitiesp()P ()p(vi | xt , zt )p(m | xt , zt )p(Sx )P (Sx = 1 | Mr )P (Sx = 1 | r, θ, ϕ)P (r, θ, ϕ | Mr )probability distribution
discrete probability
probability of voxel state given sensor state and measurement
probability of map given sensor state and measurement
probability of occupancy state of the world point x
occupancy probability of world point x given measurement
occupancy probability of world point x given true position
probability of true position given measurement

Acronyms and Abbreviations
FOV

Field Of ViewxChapter 1

Introduction
1.1

Motivation

Underwater robotics has regained the attention of researchers around the globe in
the last few years, after it being pushed down the priority list by aerial and land
robotics. Throughout the last two decades of the twentieth century, underwater
robotics experienced a boost in terms of hardware development, software solutions
and mapping approaches. Many of the mathematical fundamentals were derived inthat period and more elaborated software solutions came in harmony with hardware
developments, such as novel sensors. However, due to unforgiving test conditions
under water, including salinity, high pressure and unforeseeable fluid motion, many
publications from that time are purely theoretical, with the proof of concept inpractice pending. A wider range of possible applications and easier test conditions
shifted the focus more and more towards aerial and ground robotics.
Since 2000, the cost for land and aerial depth sensors decreased significantly. With
commercially available Lidars and depth cameras, such as Microsoft Kinnect [1],the focus in autonomous robots shifted heavily towards areas where these sensors
could be used. With high resolutions, good frequencies and unseen affordability,
these sensors opened an entire field of new use-cases. Particularly the developmentof drones experienced high interest from research institutions and economic actors
alike. During this time, many new mathematical frameworks for mapping, localisation and path planning were introduced and optimized for real-time operation.
For underwater robotics however, these innovations were somewhat irrelevant bythat time, as none of these sensors could be used underwater. Most of the newer
sensors worked with some sort of light on the time of flight principle. Under water,light is diffused and deflected at floating particles, which creates noise signals. Additionally, due to refraction and high attenuation rates of light waves (particularlyin the infra-red spectrum) in water, the penetration distance very low compared tothe one in air. This means that light-based sensor feedback is often hard to use, if
not impossible at all. A ready solution for that problem are sound-based sensors,
such as sonars. Thanks to lower wavelengths, sound can penetrate water for higher
distances, even in high turbidity. But their low resolution and comparably large
size make them not an attractive solution for aerial or ground applications.
After 2015, more and more researchers focused again on underwater robotics and thenumber of publications increased again. Possible use-cases for underwater robotics
were found to be routine inspection tasks of man-made structures (such as harbour
infrastructure, pipelines or buoys), topography analysis and mapping, mine detection and elimination, seafloor examination, and ship hull inspections, among others.
Therefore, as well an economic interest in the research of underwater robotics devel1

Chapter 1. Introduction

2

Figure 1.1: Predicted market potential for underwater robotics derived by Verified
Market Research [2].
oped, where large companies, such as offshore platform operators, were interested in
the results. Figure 1.1 shows the predicted market potential for underwater robotics
in the period of 2020 to 2028 to grow strongly. Most of the growing potential comesfrom the oil and gas sector and the rising of national security issues [2]. In thelast few years, cheaper and better underwater sensors, among them side-scan andforward-scan sonars, became available and allowed for more elaborated mapping
and navigation techniques. Additionally, different types of specialized underwater
robots were introduced, design for very specific tasks. [3] [4] [5]Today, the area of underwater robotics profits strongly from the developments andachievements in the fields of aerial and land robotics during the 2000s, as a lot ofknowledge can now be transferred to these platforms and sensors. Still, light-based
sensors are hard to use underwater and the main focus lies on more and more precise sonars. But the mathematical frameworks and algorithms derived for mapping,localisation and path planning above water can now be adapted for the usage underwater. Online volumetric mapping and localisation in real-time can now be considered the bottleneck for the further exploitation of underwater robotics. With
this problem solved, autonomous or tele-operated underwater robots could becomea very valuable asset for the use-cases described earlier. Not only are they able todive deeper than human beings, but they have longer operation times, no decompression time, more sensors for environmental perception and allow to build mapsof so far unknown regions.
An example for an emerging company in the field of underwater robotics is Tethys
Robotics [3], who build an underwater platform equipped with a multitude of sensors for fresh water body inspection. Section 2.1 will introduce the platform in
more detail. One of the promising use-cases for this platform is the detection of
ammunition disposals in lakes. This thesis uses information and sonar imagery from
Tethys Robotics.

3

1.2. State of the Art

1.2

State of the Art

1.2.1

Underwater Mapping

As described in the motivation section 1.1, many of the current implementations
for underwater robotics are based on achievements in land and aerial robotics. In
general, mapping serves a specific purpose and thus, the approach to build themap is chosen adequately. Mapping techniques exist for feature detection, surface
reconstruction, navigation or localization tasks, to name a few examples. This work
focuses on volumetric mapping for navigation. On a basic level, surface mapping anddirect volumetric representations are closely related, as both focus on the detection
and representation of objects. Good surface reconstruction allows to build maps fornavigation as shown by Oleynikova et al. [6].
In the following, the state of the art with respect to general volumetric map building
will presented. This gives an insight into the current challenges and approaches andshows their advantageous and weaknesses. After that, some of the most relevantpublications covering occupancy and sonar mapping will be shown.
Map Building
Depending on the complexity of the map building process and the available computational power, there are different types of map representations. Hornung et al. [7]introduced one of the most commonly used ones called Octomaps. By storing map
information in a tree like structure, the access time and storing capacities are significantly better than for older storage methods. Other representations use bucket-likestructures for storage [6] [8], which seem to perform even better than octrees andallow to expand maps on the go. However, most publications do not mention aparticular storage method, which suggests, that storing the vanilla way is common[4] [9] [10]. All the methods share that they store information on a section of theworld, which can contain information about colour, occupancy, surface connection
et cetera. This information has to be extracted from the sensor output.
In general, maps can be created either online on the vehicle or offline with external computational power. Additionally, the map generation can be performed inreal-time or postponed. Literature offers many insights in the advantageous anddisadvantageous of the different types of map generation. An algorithm for a point
cloud generation with imagery from a multi-beam sonar with an acoustic lens isshown by Cho et al. [4]. The algorithm does not work in real time and uses external computational power. Their process produces reasonably good results for aplanar seafloor assumption with distinguishable objects. Fallon et al. [5] presented
an online real-time algorithm that runs on affordable hardware, creates a local map
and compares it to a given global map for localisation. As their platform is intended
to be destroyed in the process of underwater mine countermeasures, the computational power and sensor quality is very limited. Due to these limitations and therequirement of a global map for localization, the method is rather specialized andrequires the robotic device to break the surface every now and then to relocate withGPS.
Occupancy and Sonar Mapping
Volumetric occupancy mapping focuses on a representation of the world that states
which areas are occupied and which are free. As discussed earlier, volumetric mapping and surface reconstruction are closely related.
Curless and Levoy [11] presented already early a predecessor of truncated signed
distance fields (TSDFs) to find occupied sections of the world. Newcombe et al.

Chapter 1. Introduction

4[12] extended this work and used the then newly available Microsoft Kinnect sensor to introduce a more complete surface reconstruction with the help of TSDFs.
Oleynikova et al. [6] introduced Voxblox, a library that builds Euclidean signed
distance fields (ESDFs) from TSDFs and outperforms most of the currently implemented algorithms in terms of speed, computational cost and map quality. And
finally, Loop et al. [9] used elements from TSDFs and occupancy maps to build up
one of the most elaborated mapping processes up to date.
Volumetric mapping from sonar imagery comes with a special set of challenges.
Sonar images summarize information from a sphere cone onto an image plane.
Therefore, the recovery of lost elevation information, and thus the extraction ofgood features in three dimensions, is generally one of the biggest issues to solve.
Several techniques have been introduced. Guerneve and Petillot [13] suggested touse multiple views of the same object and pose a motion constraint on the platform
to move along the direction with the highest uncertainty. DeBortoli et al. [14] tooka step back and introduced a convolutional neural network (CNN) to only selectmeaningful images for the upcoming information recovery. In another publication,
DeBortoli and his colleagues [15] used again a CNN to estimate the missing elevation
information from one image only. Teixeira et al. [10] tried to avoid the recovery of
the elevation information completely by using a forward-looking sonar with a strong
lens that reduces the field of view (FOV) to almost a line. But the probably mostused technique to recover elevation information is to use acoustic shading [4] [16][17] [18]. The calculation involves assumptions on the seafloor orientation and uses
geometrical properties of the shadow orientation combined with a known vehicle
pose.

1.2.2

SLAM

With increasing computational power on mobile platforms, the formerly separated
problems of mapping and localization are now often addressed at the same time inform of a Simultaneous Mapping and Localisation (SLAM) formulation. A standard
SLAM map only contains feature points and does not provide additional information such as occupancy. It has to be mentioned that these maps are suitable forthe localization tasks they are designed for. However, in comparison to occupancy
maps, they are rather unpractical for path planning. For aerial and land robotics,
convincing solutions have been published and implemented in the last decade, leading to a growing autonomy for these vehicles [19].
For underwater robotics, the SLAM formulations are yet to mature. Particularly
the low localization precision under water due to the lack of GPS, the poor map
quality and the unpredictable motion of the water make SLAM significantly harder.
Most of the present implementation try to account for the aforementioned problems
with more or less complex motion models. For example, Ribas et al. [20] createda SLAM framework for forward-looking sonars that extract manmade line-featuresfrom rectified images. Li et al. [21] implemented a pose-graph based SLAM method,that uses bundles of images to account for motion drift. The mapping part is reduced to feature detection and matching. And finally, Teixeira et al. [10] introduced
a submap-based SLAM techniques that builds smaller submaps and puts them together for the global map. Through scene registration the motion drift is partiallycompensated.
All current methods would benefit largely from a mapping framework that provides
the localization part with a precise and reliable map. From the literature it becomes
obvious that the high uncertainties in the pose and the sensor readings are one of
the main contributors to poor results.

5

1.3. Problem Statement

1.3

Problem Statement

The creation of 3D-maps is a crucial part for any type of interaction task between arobot and the environment. For aerial and land robotics, the usage of lidars is very
common to build volumetric maps of the surrounding of a robot. Due to reflections
at floating particles and high refraction factors in turbid water, light-based sensors
fail to provide reliable information on the surrounding, particularly on distanceshigher than a meter or two. An alternative are sensors based on sound, such as
sonars. The lower frequencies of sound can penetrate turbid water a lot better thanlight and allow for usage at higher distances. On the downside, sonars do no longer
provide volumetric point clouds but provide the user with a 2D image, similar toa camera. Additionally, they come with much lower resolutions than light-based
sensors. In summary, the following core problems occur for volumetric mappingwith sonar imagery: Memory efficient map storage and maintenance Elevation information recovery from 2D sonar imagery
 Feature/response detection in sonar imagery (e.g. noise rejection)
 Cost-efficient update calculations

As the state of the art section 1.2 showed, there are already promising approaches
to these core problems. In this thesis, the current approaches will be analysed andon the basis of the results thereof, a novel approach will be derived to tackle thecore problems.

1.4

Goal

This work aims at introducing a complete sensor model for volumetric mappingwith imagery from forward-scan sonars. With this model, an occupancy map shallbe built that contains occupancy probabilities for visited parts of the world. Finally, this occupancy map will be put into the pipeline of the mapping frameworkof Voxblox [6], which allows path planning and finally SLAM with cutting-edge
performance.

1.4.1

Thesis Structure

In a first step, research results on various relevant topics will be presented in the
research chapter 2. The mathematical fundamentals needed will be presented in
3. Then, the sonar image will be closer examined and image preprocessing will bedefined in 4. Following that, the gained knowledge will be used to create a sensor
model 5. Using the sensor model, formulations for an occupancy map representation
will be presented in 6. The implementation in voxblox provides an overview over the
performance of the new sensor model presented in 7. Finally, a brief summary anda discussion 8 put the results in relation to the formulated goal and the intended
improvements.

Chapter 1. Introduction

6

Chapter 2

Research
2.1

SystemThe process of mapping from sonar imagery is not only limited by the resolution andquality of the imagery, but as well by the precision of the pose estimate of the sonarin a global coordinate system. The details will be further explained in the mapping
section 2.3, but this dependency introduces some requirements on the platform thesonar is mounted on. Particularly, the mapping process needs extra informationfrom the platform, such as a motion estimate, the height above seafloor and thepose with respect to a fixed world origin. In case of a SLAM setup, the latter isusually not available. The mapping process takes the information from the platform
to determine the general state of the platform and then uses the sonar imagery tobuild a map from the environment of that state. For this thesis, the platform isprovided by Tethys Robotics [3] and contains the necessary sensory equipment.
Tethys Robotics is a soon-to-be start-up from ETH Zurich, which is dedicated tobuild an underwater robotic vehicle for inspection tasks in freshwater lakes. Theircurrent platform prototype likes a research submarine, which is remotely controlled.
Moved by thrusters at a maximum speed of around 2.5 meters per second, theplatform is optimized for the usage in lakes and slow-moving rivers. It is equippedwith flash lights and RGB cameras for inspections in clear water, but offers as well aBlueprint Subsea M1200d sonar [22], which will be inspected closely in 2.2. The pilot
uses the camera feedback for navigation and object recognition. A simple motion
estimator is implemented using inertial sensors. For the detection of the height
above lake floor, a Doppler Velocity Log (DVL) is used. The current prototype isequipped with a Short Baseline (SBL) acoustic positioning system, which provides
the approximate position of the vehicle with respect to a GPS-fixed point above
the water level. All these inputs are fused into a state estimator that estimates thepose and motion of the vehicle in real time. For this thesis, the state estimator isassumed a fixed input variable to the mapping process.
One of the biggest issues of platforms operating close to the sea floor for extended
periods of time at the same place, is the mud that is moved by the thrusters. These
floating particles are lightweight and move thus easily from the ground but stayafloat for a long time. Once happening, the camera give very poor imagery due tothe light being reflected and diffused by the particles. Even though the platformfrom Tethys has their thrusters mounted in an optimal angle, this problem canoccur when moving close to the lake floor. Additionally, the presence of algae andfloating particles in Swiss lakes is rather high, which leads to a very limited visibility
in certain areas.
7Chapter 2. Research

8

Figure 2.1: CAD-imagery of the current prototype of Tethys Robotics called
Protheus. It is equipped with standard sensory and is tele-operated. The communication is performed through cable. [3]

2.2

Sonar

As described earlier, light-based sensors are hard to impossible to use in turbid
water. Sensors working with lasers suffer from reflections at floating particles andrefractions in the water, which leads to very noisy and unreliable data. Camerason the other hand suffer from diffraction and refraction of light, with poor image
quality as a consequence. In greater depths, cameras need additional light sourceson the vehicle to get enough visibility. In turbid waters, the range of these lightbased sensors is restricted from centimetres to a few metres. As this is not suitablefor precise mapping and localisation tasks, active sonar sensors come into focus.
Active sonars have a sound emitter and a receiver, thus, they create their own
signal. With frequencies in the range of kHz to MHz, they come with much lower
frequencies than light-based sensors and can avoid diffraction and refraction to acertain degree at the cost of a noisy image with low resolution. In general, sonars
are distinguished by their orientation on the vehicle, their beam pattern and thescanning range. The most prominent types of sonars are side-scan and forward-scansonars.

2.2.1

Side-Scan Sonar

Side-scan sonars are probably the most used sonar type up to date and are often usedon boats and torpedo-shaped underwater vehicles. They are mounted orthogonal
to the direction of motion looking vertically on the seafloor. Usually, they send asingle line beam, thus, the sonar has no vertical opening angle. An image of theseafloor is created by stacking several subsequent line-scans. The image is often
of good quality and allows for seafloor inspection in top view. However, due tothe way images are built up, they have motion blur for moving objects. The mostsignificant limitations come from the fixed observation angle and the fact that each92.2. Sonar

Figure 2.2: Example imagery of a bicycle on the seafloor from a high quality sidescan sonar. Image source: [23]section of the world is only seen once due to the line characteristic of the beam.
As a consequence, they are most often used as fish finders or for general seafloor
examination, for example to find objects or examine structures from distance.

2.2.2

Forward-Scan Sonar

Forward-scan sonars are specialized devices for the usage on underwater robotic
platforms. They are designed for slow-moving vehicles that navigate close to seafloor
or inside structures. Their orientation is usually aligned with direction of motion
and tilted by around 15 degrees towards the seafloor. This way, they provide imagery of what lies ahead of the robot and can gather information from the seafloor.
Most forward-scan sonars are multi-beam sonars, meaning, that a single image consists of a number of different beams at the same time. The number of beams varies
between different models, but 256 beams is an often used standard. Each beam issimilar to a line-scan in the vertical direction. Therefore, the sonar image shows
information gathered from a sphere cone in 3D space. Figure 2.3 shows the working
principle of a multi-beam forward scan sonar graphically. As the sonar image is
two-dimensional, the image loses the elevation information of the reflection. Moredetails on this process will be given in the sensor model chapter 5. Thanks to thesonars position on the vehicle, it can be used to build up detailed maps of theseafloor or of structures the vehicle moves in. Forward-scan sonars are often usedin closer range inspection tasks and map building.
In this work, simulated and real data from a forward-scan sonar will be used, mainly
from the Blueprint Subsea M1200d [22]. The datasheet can be found in the Appendix A. This sonar has two modes. The first with frequency 1.2MHz has a wider
opening angle and results in range up to 30m. The other offers a higher angularresolution with 2.1MHz frequency at cost of a narrower field of view and a range
up to 10m. Typical for forward-scan sonars, the image is noisy and does not offer
elevation information.

Chapter 2. Research

10

Figure 2.3: The image is built from multiple beams and summarizes reflections froma sphere cone as indicated in the image on the left. A multi-beam forward scansonar is usually mounted in direction of motion and tilted towards the seafloor as
shown on the right. [24]Figure 2.4: On the left, the Blueprint Subsea Sonar M1200d [22]. On the right animage of a folding chair on the lake floor taken with this sensor by Tethys Robotics.
Opposite to a camera image, the sonar is placed at the top of the image.

2.3

Mapping

The mapping is a complex and error-prone process, whose results are heavily dependent on the input quality and the approach chosen. For a regular mapping
approach, the position of the sensor is known in a global coordinate system. For
example, a drone uses GPS to determine its position in a world frame and thenuses the sensor imagery to build a map from the place defined by its own global
coordinates. Another possibility is the creation of a local frame by a motion tracking system, that again allows to determine the position of the vehicle, and thus thesensor. For aerial and land applications, these techniques work reasonably well andmaps can be created with resolutions up to a few millimetres [12]. For underwater
robotics however, motion drift is a big issue in terms of mapping precision. Standard GPS is not available underwater, as the signal can not penetrate the water
surface [25]. Therefore, motion tracking through sensor readings or scene registration remains one of the only approaches to counter the present motion drift. Its
implementation is, however, costly and the technical possibilities are limited. As aresult, the mapping pipeline needs to take the state uncertainty into account andit is clear that the better the state estimation is, the higher a map quality can be
expected.
For all types of autonomous systems, there are cases, in which no position information with respect to a global frame is available. This includes scenarios, where112.3. Mapping

this information would theoretically be obtainable, but for the lack of sensor equipment, it can not be used. For these cases, a Simultaneous Mapping and Localization(SLAM) formulation is needed. The algorithms of that field combine the problemsof building a map and locating the vehicle in it. Particularly in mobile autonomous
robotics, the efficient solution to the combined problem can be considered the holygrail these days. For underwater robotics, the first few implementations of SLAM
algorithms have emerged, such as the ones presented by Ribas et al. [20], Li etal. [21], Fallon et al. [5] and Teixeira et al. [10]. Most of them adapt techniques
well known from aerial and land robotics, but prove themselves often unstable andunprecise. Independent of the availability of the localization through the knowledgeof global coordinates or through onboard estimation, the mapping process stays at
the core of the problem to solve.

2.3.1

World Representation

Maps of the 3D world need a digital representation that allows to perform algorithms on. Similar to 2D maps and images, the 3D world is usually discretized intoelements with constant size. Most mapping algorithms introduce cubic elements
called voxels, which are the equivalent to pixels in three dimensions. A voxel always carries the position of its centre. Additionally, it can contain other properties,
such as occupancy, colour, material or distance to closest surface. Probably the best
way to illustrate voxel properties is through video games like Minecraft. From thecharacters perspective, one looks into a world scenery (see figure 2.5 as an example)
very similar to one built from voxels. Each voxels contains its position, but as wellmaterial, colour and interaction properties.
The side length of a voxel defines the resolution of the map, as all information of the
cube is summarized at its centre point. This introduces one of the most important
trade-offs for mobile platforms with limited computational resources. The smaller
the voxel size, the better the resolution and the map quality, but the higher thecomputational cost for map updates and planning algorithms. This often leads tomaps in worse resolution than the sensor returns would actually allow, traded forthe possibility to build the maps in real-time and online.

Figure 2.5: An example scenery from the video game Minecraft. The cubic setupof the world is clearly visible and is very similar to voxel-based maps from sensor
input. Image source: [26]Chapter 2. Research

12Computational and memory cost are not only dependent on the map resolution, but
as well on the map size. The size of many maps is predefined, an example for thatare Octomaps [7]. This works well if the portion of the world, where the mapping
should take place, is known in advance or if the platform has motion limitations.
Assuming, for example, a maximum flight distance of two kilometres for a drone, thesize of the map can easily be pre-determined as a circle with two kilometres radiusaround the zero position of the drone. However, this leads to a memory allocation,
which is much larger than actually needed. Due to this, and for use-cases such as
search and rescue missions or mapping in completely unknown environments, themap size might have do be expanded on the run. There are different approaches
to this in literature, but Niessner et al. [8] presented one of the most common
ones. They work around the map size limitation by using voxel hashing, a memory
allocation technique using aligned buckets to store voxel centres. However, there isan upper bound for the map size in any case, as the computational power and theavailable memory are limited. Oleynikova et al. [6] introduced a distance threshold
between the sensor and the edge of the map to remove sections again that are
far away from the current position. In general, the adaptive size of maps allows
for a more general definition of the use-cases and is therefore more attractive for
implementation.

2.3.2

Surface Reconstruction

In mobile robotics, inspection tasks are one the strongest growing fields, as a large
economic interest is present. The goal in inspection tasks is to provide the best
possible representation of an object to the operator (human or AI). Curless andLevoy [11] introduced a predecessor of truncated signed distance fields (TSDF) as
early as 1996 as a surface reconstruction method from range measurement point
clouds. TSDFs are used up to date for surface reconstructions, as Oleynikova et
al. [6] show with their work from 2017. On the downside, the surface reconstruction process at high resolution is computationally rather expensive, especially forlarger areas. Therefore, surface reconstruction at millimetre resolution is usually
performed in small environments, such as rooms. Figure 2.6 shows the results from
Newcombe et al. [12] for an environment smaller than 7m3 .

Figure 2.6: Surface reconstruction in millimetre resolution of small room achieved
by Newcombe et al. [12]13

2.3. Mapping

For underwater robotics, Lidar and depth cameras are hard to use, as explained in
the motivation section 1.1. The poor image quality compared to the aforementioned
sensors makes surface reconstruction at similar resolutions close to impossible. So
far, most of the research conducted focuses on occupancy mapping under water.
However, the future might hold a new generation of 3D sensors for underwater
usage that could enable more research in that direction.

2.3.3

Occupancy Mapping

In occupancy mapping, the focus lies in the representation of free and occupied
space. Very often, occupancy maps ignore other properties of the world, such as
colour or material. Either, because the platforms are not equipped with sensors thatprovide this information or because the information is simply not needed. The goalof this mapping technique is to build a map that shows as accurately as possible
where objects lie. Figure 2.7 illustrates the functionality of an occupancy map in
2D.
In comparison to surface reconstruction techniques, the exact surface shape is not as
relevant. Due to the poor image quality from sonar sensors and high uncertainties
in state measurements, surface reconstruction through smoothing and interpolation
is difficult. However, most path planning algorithms that rely on occupancy maps
to find the best path from one point to another, do not require precise surface
information.

Figure 2.7: An occupancy map in two dimensions showing three different states
for pixels, namely free (white), unseen (grey) and occupied (black). The range
boundary can be implemented to be either of these states. The final map is thenbinary, representing the worlds true state of occupied or free. [27]Chapter 2. Research

2.4

14

Voxblox

Voxblox is a mapping and path planning library first presented in 2017 by Oleynikova
et al. [6] at ETH Zurich. The library is specifically build for mapping tasks withdrones, where at the time, GPUs were rarely available. Therefore, the entire libraryis built for CPU only. On a theoretical level, they use Truncated Signed DistanceFields (TSDFs) to build Euclidean Signed Distance Fields (ESDFs). ESDFs holddistance information to obstacles. In a nutshell, a voxel contains the Euclidean
distance to the next occupied voxel. This type of information is very useful forpath planning, as the complex process can be reduced to a maximization problem.
Figure 2.8 shows the workflow of Voxblox and figure 2.9 displays an ESDF field
for path planning. Up to 2017, ESDFs were usually built from Octomaps. However, Octomaps come with some limitations, for example, the map size needs to be
known a priori and they rely on the octree data structure. In Voxblox, these ESDFs
are built incrementally from TSDFs. This allows to expand the map on the run
through an underlying data structure called voxel hashing. On the performanceside, the presented algorithm is faster than comparable techniques building ESDFs
from Octomaps. Furthermore, the quality of the created map is better.

Figure 2.8: Voxblox uses sensor information and the state estimator output to build
TSDFs and then builds ESDFs incrementally from them. The output then serves
for visual inspection, localization and path planning. [6]However, Voxblox has a few issues that prevent it from getting a standard libraryfor other projects. First, as it is typical for most distance field representations, thealgorithm does not work in a probabilistic way and ignores all uncertainties. Voxels
are, for this type of maps, usually considered spatially independent and uncertainties are difficult to handle in this context. The good quality of the derived map
proves that the assumptions made are justified to a certain degree for the use-casepresented by Oleynikova et al. This is mainly due to the very well defined pose of
the drone (little pose uncertainty) and the high accuracy of the used Lidar sensor.
It has to be mentioned though that Voxblox uses voxel sizes of up to 20cm, whichis, compared to other techniques using Lidar sensors, a very low resolution. This,and the missing uncertainty quantification, leads to problems with slim and small
objects, such as flagpoles or small pillars. As a consequence to the missing uncer-

15

2.4. Voxblox

tainties, each measurement of a point is regarded as “true”. Even though there isa weighting system, many measurements of the same point lead up to a significant
number of contradicting measurements. This can result in vanishing objects or thecreation of artefacts, thus, non-existing objects.
For underwater robotics, the current implementation of Voxblox is not usable inpractice. The assumptions made to justify the ignorance of uncertainties and noise
do not hold for sonars by any means. As described in the mapping section 2.3,
the pose is often biased by significant error and the sensor measurements are very
noisy. As a result, the presented library has to be adapted for underwater usage.
In a certain sense, the algorithm has to be generalized to be not only a valid option
for Lidar sensors, but as well for other sensor types with higher uncertainties. One
way to do this is by replacing the TSDF derivation with a probabilistic occupancy
mapping. While this would remove the implicit surface reconstruction characteristic, it would allow to take all given uncertainties into account. Additionally, aprobabilistic approach has higher chances of grasping small and slim objects, as
individual measurements are fused in the current assumption of the world. The
resulting occupancy map can then still be used in the pipeline to derive ESDFs anduse the path planning properties of the current implementation.
In summary, the current implementation of Voxblox has big potential for high quality maps and excellent path planning capabilities. However, the algorithm has its
problems with small and slim objects and creates artefacts over a large numberof measurements. For underwater robotics, the current implementation has to beadapted to take uncertainties and noise into account.

Figure 2.9: An ESDF layer used for path planning displaying distance propertiesof voxels in the colour code. [6]Chapter 2. Research

16Chapter 3

Mathematical Fundamentals
In this chapter the mathematical fundamentals needed for the derivation of the sensor model will be introduced. This includes the sonar basics and an introduction toprobabilistic occupancy mapping. Furthermore, crucial definitions and assumptions
will be made upon which the sensor model is built.

3.1

Sonar Basics

In section 2.2, the different types of sonars were presented. As explained, a multibeam forward scan sonar is used as the primary sensor for the upcoming mapping
process. In the following, some of the main characteristics and definitions for thistype of sensor will presented. The focus lies on information that is needed for thesensor model derivation and the selection shown here is not complete.
In a first step, the coordinate system of the sensor and therefore of all subsequentcalculations has to be defined. The choice of coordinate system is often a questionof intuition and it aims at the representation with the easiest calculations. Dueto the functionality of a forward scan sonar, the most practical representation of
the world is given in spherical coordinates, with radial distance r, azimuth angle θand elevation angle ϕ. The image plane is two-dimensional (elevation information
is lost), represented by range r and azimuth angle θ, withp
x2 + y 2yθ = arctan( )xρ=(3.1a)(3.1b)

transformed from cartesian coordinates. Figure 3.1 displays the spherical coordinates with the projection onto the image-plane. Each image I consists of N measurements zn , whereN=

opening angle
angle increment per beam

(3.2)

is the number of beams. Therefore, the image I is built from measurement vectors
zn and can be described as

17

Chapter 3. Mathematical Fundamentals

18

Figure 3.1: The spherical coordinate system and the projection onto the image
plane with coordinates ρ and θ.



z1,1
 z2,1
Z = {z1 , z2 , ..., zN } =  .
 ..

z1,2
z2,2
..
.

...
...
..
.


z1,n
z2,n ..  , with zn = zn (θ)
. 

zm,1

zm,2

...

zm,n

(3.3)

Given the robot state, the individual measurements are assumed to be independent. This assumption is not entirely true, as crosstalk between the beams is oneof the noise sources. However, by taking appropriate countermeasures to noise, theassumption of independence is valid. Expressed mathematically, the probability ofan image given the world P (I | X) is given by

P (I | X) =NYP (zn | X)

(3.4)

n=1

Each measurement zn returns an array of m bins (pixels) centred at (rb , θb ) andwith sides δr and δθ. Each bin collects the reflections of all points lying on theδrδθδθ
sphere segment (r, θ, ϕ) with r ∈ [rb − δr
2 , rb + 2 ], θ ∈ [θb − 2 , θb + 2 ] and
ϕmax
ϕmax
ϕ ∈ [− 2 , 2 ]. The reflected energy collected in one bin can be described as
Z rb + δr
Z θb + δθZ ϕmax2
2
2
I(rb , θb ) =f (r, θ, ϕ) dr dθ dϕ
rb − δr2

θb − δθ2(3.5)− ϕmax2

with f (r, θ, ϕ) the reflectance function of an object. This function includes, among
others, occupancy information, material properties and exact shape of the surface.
If the reflectance function would be known for every point in space, the state of the19

3.1. Sonar Basics

world would be completely defined. In reality, this function is unknown and could
theoretically be approximated. However, this process would be computationally
extremely expensive and would use hard assumptions on the environment. As aresult, the measurement process usually focuses only on one property of the world,
while the impact of other properties on the measured reflectance are neglected,
assumed constant or implicitly added in probabilistic approaches.
For the scope of this work, the reflectance function is simplified for occupancy
information. In a nutshell, this means that a bin collects occupancy informationof the sphere segment described earlier. A reflection collected in a bin, thus a
response in the image, can therefore originate from every point lying inside thesphere segment.

3.1.1

Sonar Pose

The vehicle pose, including the position and orientation, is a state received fromthe state estimator and serves as an input to the mapping process. As the pose
estimation is not perfect, an uncertainty is associated to it. From the mapping pointof view, the vehicle pose can be seen as a measurement with noise. This replicatesthe uncertainty in the pose and allows for further calculation. The true pose vector
is given by
 
X
Y 
 
Z 

X=
A
 
B 
Γ

(3.6)

with A, B and Γ corresponding to the roll, pitch and yaw angle, respectively. Now,
the uncertainty of the pose can be modelled as a multivariate, six-dimensional Gaussian with mean µ at the true pose of the vehicle in global coordinates and covarianceΣ ∈ R6x6 . The measured vehicle pose is then given by
 
x
y 
 
z 

x=
α 
 
β 
γ

(3.7)

and thus the uncertainty by

ep = p1(2π)6 ∗ |Σ|1∗ exp(− (x − X)T Σ −1 (x − X))
2

(3.8)

In theory, this could be used to compensate the pose uncertainty in the mapping.
However, from a practical standpoint, the pose uncertainty just adds further blurring to the measurement. In other words, by not knowing exactly where the sensor
is, the position of the object creating a return in the image is even less sure. InsteadChapter 3. Mathematical Fundamentals

20of adding an additional calculation step that creates computational cost, this effectcan be elegantly approached by increasing the measurement uncertainty. This way,
the pose uncertainty is implicitly modelled in the measurement uncertainty.

3.1.2

Coordinate Transforms

For further calculations, the coordinate frame transformations are needed to findthe sensor in the world frame. First, the coordinate frame of the vehicle has to be
represented in global coordinates. This is realized with homogeneous transformations. The basic matrix is of the form

H=


R3x3
01x3

T3x1
1


(3.9)

with R3x3 the rotation matrix and T3x1 the translation vector.
In the following calculations, the global frame will be denoted as inertial frame with
subscript I, the robot frame will be denoted by the subscript R and the sensor frameby the subscript S. The complete transformation matrix is obtained by multiplying
β
γ
α
∗ HIR
∗ HIR
. Using
the three rotation matrices around the axis, e.g. HIR = HIR
abbreviations cx and sx for cosine and sine respectively leads to

cβ cγ
sα sβ cγ + cα sγ
HIR = sα sγ − cα sβ cγ
0−cβ sγcα cγ − sα sβ sγ
cα sβ sγ + sα cγ0sβ−sα cβ
cα cβ
0


Px
Py 

Pz 
1

(3.10)

Additionally, it is important to define the sensor position in the inertial (world)
frame. To this end, the transformation matrix from the robot frame to the sensor
frame has to be derived. Setting the x-axes of the robot frame to align with theheading and the z-axes pointing upwards, the y-axes is perpendicular to the robot
heading. In a standard configuration, the sonar is only tilted around the y-axes
with respect to the robot frame and translated to a position. Rotation around thex- or z-axes do not have practical usage. With sensor position and orientation in
the robot frame defined as PxR , PyR , PzR and β R , the transformation matrix reads as
cos β R0
HRS = − sin β R0

0 sin β R100 cos β R
0
0


PxR
PyR 

PzR 
1

(3.11)

Note that this matrix is constant, as all parameters are defined by the setup of therobot and do not change throughout the operation. The tilting angle β R is usuallynegative as the sonar points towards the sea floor. The pose of the sensor in theinertial frame is then just given by

HIS = HIR ∗ HRS(3.12)

21

3.1.3

3.1. Sonar Basics

Field of View

Given the pose estimate of the sensor, the Field of View (FOV) of the current measurement Zt can be derived. The FOV corresponds to the sphere cone spanned by
the azimuth angle interval, the elevation angle interval and the range interval. All
points in the FOV at time t are therefore in the volume spanned byr ∈ [0, rmax ], θ ∈ [−

θmax θmax
ϕmax ϕmax,], ϕ ∈ [−
,
]
2
2
2
2

(3.13)

with ϕ = 0 lying on the xy-plane.
To find all points lying in the current FOV at time t in the global coordinate system, the points are determined in the sensor frame and then back propagated to−1
the inertial frame by the inverse transformation matrix HSI = HIS.
Due to the shape of the sonar image cone, a coordinate system in spherical coordinates is introduced, with ϕ defined from the xy-plane instead of z-axes. Therefore,
the FOV constraint defined above can directly be implemented. The coordinatetransformation to cartesian coordinates (x in direction of heading, y in sensor planeorthogonal to x, z pointing upwards) is then given byx = r · cos θ · cos ϕ(3.14a)y = r · sin θ · cos ϕ(3.14b)z = r · sin ϕ(3.14c)The points C in the FOV in the inertial coordinate frame are then given by


CI
1



CS
= HSI ·
1


(3.15)

Chapter 3. Mathematical Fundamentals

3.2

Probabilistic Occupancy Mapping

3.2.1

Probabilistic Mapping Basics

22

Many of the following derivations are similar to the ones presented by Loop et al.
[9] in their paper on occupancy probabilities and by Thrun et al. [28] in their book
Probabilistic Robotics. As described in section 2.3.1, the map of the world is built
up by equally sized cubes called voxels. In a global cartesian frame, the voxel position is defined by its centre point through vx
ρv = vy  , with (vx , vy , vz ) ∈ (X, Y, Z)
vz(3.16)

In probabilistic occupancy mapping, each voxel is assigned a probability of being
occupied. This probability is updated at each time step t for all voxels in the current
field of view (FOV). To take uncertainties and errors into account, the probabilityof occupancy is calculated such that it fuses all available information. In the senseof resolution, the state summarized at the centre of a voxel is assumed true for theentire space the voxel occupies.
In a first step, the approach has to be defined. Each voxel vi has a state that
describes its properties, including position and occupancy. Let m denote the map,then

m = {vi }(3.17)

describes the map built from voxels.
The state of the sensor and of the world at time t are assumed to be fully defined.
In other words, each time step t can be understood as an initial time step, fromwhich the process could be started. As a consequence, it is assumed that the given
process can be modelled as a Markov chain. The prediction of the state at thenext time step only depends on the information available at the current time step.
This includes information such as pose and measurements. Consequently, all past
information is summarized in the current statep(vi | xt , zt ) = p(vi | x1:t , z1:t )(3.18)

Expanding the definition of the voxels, an independence constraint among the voxels is imposed. This is a rather strong assumption in terms of information rejection.
For example, by looking at an image of a wall, humans can deduce that if all points
around a certain point are part of the wall, that the given point is very likely to
be part of it as well. While there is a lot of potential in these deductions, they arecomputationally very expensive, as the interdependencies avoid parallelization in thecalculation. Additionally, the probability derivation for a voxel can get infinitely
complex, depending on the definition of the relevant neighbourhood. Consequently,
this justifies the independence constraint and allows to build the map by fusing all
voxels into

23

3.2. Probabilistic Occupancy Mappingp(m | xt , zt ) =Yp(vi | xt , zt )(3.19)

i

Note that this independence constraint among voxels is not per se true for the realworld. A measurement zi might not only be depending on the corresponding world
point, but on the entire world. With the general map characteristics defined, acloser look at the specific case of occupancy can be done.

3.2.2

Occupancy Mapping Basics

Occupancy maps are a specific type of probabilistic maps, which focus on the occupancy information. In general, a voxel does not only hold its position, but aswell its occupancy state as an attribute. In the following, the fundamental setup
of occupancy maps will quickly be explained and the update space for probabilities
will be defined.
StatesIn the real world, a point x ∈ R3 can be either occupied or free. Let S ⊆ R3 be arandom set and
(
1
Sx =
0

if x ∈ S
if x ∈/S(3.20)

the occupancy at point x. The priors p0 of the two states are assumed to be equally
likely, thus

p0 (Sx = 0) = p0 (Sx = 1) =1
2

(3.21)

Depending on the environment, such as airspace, this assumption is probably notvery accurate. However, an unbalanced distribution would lead to a heavy-tailed
conclusion, which is not intended. After all, the probabilistic approach already covers the uncertainties of the environment and the sensor.
In occupancy mapping, voxels contain a continuous representation, namely the likelihood of occupancy. Voxels are initialized by the prior probability of being occupied,which is 12 . By using observations and the sensor model, the likelihood of occupancy
tends to either 0 or 1, and thus to the free or occupied state. In summary, the binary state of the real world, free or occupied, is represented as a likelihood in themap. Which state is assigned to a voxel is depending on the threshold, above whicha likelihood is considered strong enough.

Chapter 3. Mathematical Fundamentals

24

Binary Bayes Filter with Static StateAs the two possible states for points in the real world, free and occupied, don’tchange over time, the problem can be formulated as an estimation problem withstatic states. The solution to this problem can be found by a Binary Bayes Filter.
Fundamentally, the two states are exclusive, meaning, that only one or the othercan occur asp(Sx = 1) = 1 − p(Sx = 0)(3.22)

Let p(Sxt | zt ) denote the occupancy probability given the measurement zt at time t.
Then, the standard update for the binary Bayes filter using a static map assumption
is given byp(Sx | zt+1 ) = η · p(zt+1 | Sx ) · p(Sx | zt )(3.23)

with η a normalization constant derived from the priors to keep the probability
bounded. In words, the formula calculates the current occupancy probability bymultiplying the previous one with the current probability of the measurement given
the true occupancy state and normalizing the result. An elegant way to avoid thenormalization is to move the update to the log-odds space.
Log-Odds
The odds of a state Sx are defined as the ratio of the probability of the event and theprobability of it not happening. Following the exclusion theorem above, it follows
that

odds(Sx ) =p(Sx = 1)
p(Sx = 1)=
p(Sx = 0)
1 − p(Sx = 1)

(3.24)

While regular probabilities are defined in the range [0, 1], the odds has now definedin [0, ∞]. It is obvious that this ratio is ill-defined at event probabilities close to 1.
To allow simple summation update of probabilities, the log-odds is introduced asl(Sx ) = ln odds = lnp(Sx )
1 − p(Sx )(3.25)The log-odds is basically a coordinate transform, which shifts the scope from [0, 1]
to [−∞, ∞]. In log-odds space, probabilities can just be summed to update. Letl() denote the log-odds function. Applying the log-odds transform to equation 3.23,
the multiplications turn into sumsl(p(Sx | zt+1 )) = l(η) + l(p(zt+1 | Sx )) + l(p(Sx | zt ))(3.26)

25

3.2. Probabilistic Occupancy Mapping

The transformation allows to update the voxels occupancy state incrementally withevery new measurement that offers information for the respective voxel. For theconstant state priors defined in 3.21, the log-odds is given by

l0 = ln1p0 (Sx = 0)
p0 (Sx = 0)
= ln
= ln 2 1 = 0
p0 (Sx = 1)
1 − p0 (Sx = 1)
1− 2

(3.27)

This is coherent as with this choice of prior no initial off-sided probability was given.
Therefore, the update formula simplifies tol(p(Sx | zt+1 )) = l(p(zt+1 | Sx )) + l(p(Sx | zt ))(3.28)For voxels not in the current view, thus voxels, for which no measurement was received, the update is trivial and given byl(p(Sx | zt+1 )) = l(p(Sx | zt ))(3.29)The transition back into probability space can finally be derived for the general caseasp(Sx ) = 1 −1
1 + exp(l(Sx ))(3.30)

Chapter 3. Mathematical Fundamentals

26

Chapter 4

Image Preprocessing
It is well known that sensor models and image analysis algorithms work best withnoise-free images, where each response is a true signal. However, sonar imagery
is often very noisy and might have additional artefacts due to internal sensor inaccuracies. To still receive reasonably good results from the sensor model, image
preprocessing is needed. This includes first an analysis of the original imagery andthen suitable countermeasures to improve the image quality as much as possible
with the resources available. As the image quality is highly dependent on factors
such as depth, water quality or floating particles, the following analysis is more
representative than absolute. Figure 4.1 shows the example image of the folding
chair on the lake floor used in the analysis.

Figure 4.1: The original image of the folding chair on the lake floor used for thefollowing image analysis. [3]27Chapter 4. Image Preprocessing

4.1

28

Noise Rejection

The imagery from the forward scan sonar is an 8-bit grey-scale image and offers
thus 256 intensity levels. Noise can have various sources in sonar imagery, such as
crosstalk between beams, foreign sound sources and internal sensor noise. However,noise created through crosstalk is by far the most occurring. To find the range ofnoise in the imagery, the intensity values have to be analysed.
One of the most intuitive ways to find the range of noise is by plotting an intensity
histogram. Figure 4.2 shows the histogram of the chair image. The histogram displays that the large majority of returns lies in the lower range of intensity values,
somewhere between zero and 50. The average return intensity is not higher than
13. It is reasonable to assume that direct reflections give high intensity returns,
at least at low to medium-large distances. This suggests that many returns in thelower intensity range might result from crosstalk.

Figure 4.2: The intensity histogram showing a high number of signals in the low
intensity ranges.
One way to prove the aforementioned assumption is to set all returns lower thana certain threshold to zero. If the assumption holds, this flooring operation should
enhance the contrast, while the main features should be preserved. Figure 4.3 shows
the original image and the floored images for thresholds between 10 and 50. The
floored images show the expected behaviour. The key features are still clearly visible while the contrast is in fact enhanced. The behaviour is directly correlated tothe threshold value. It has to be mentioned that by flooring the image the computational cost for the occupancy calculation in a frame is drastically reduced. A
threshold of ten already reduces as much as 66% of the returns, a threshold of 50
over 95%. It is obvious that a higher thresholds leads to more rejection of important
information. Therefore, the tuning of the threshold becomes a trade-off decision between noise rejection and information maintaining.
In summary, flooring seems to reduce the amount of noise due to crosstalk significantly and allows to reduce the number of active returns. This way, the sonar image
keeps its key features, while the computational cost per frame is much lower due tolesser non-zero returns.

29

4.1. Noise Rejection

Figure 4.3: The floored images compared to the original at the top left. Thresholds
are defined as 10 (top right), 20 (centre left), 30 (centre right), 40 (bottom left) and50 (bottom right).

Chapter 4. Image Preprocessing

4.2

30

Image Downsampling

The image size from the forward scan sonar Blueprint Subsea M1200d is 440 x 512
pixels. The resulting total of over 225’000 pixels per frame is a key contributor tolong computation times. In the worst case scenario, where all pixels have non-zero
returns, the model algorithm would have to calculate the results for every single ofthem. An additional factor is the memory that has to be allocated for each frame.
As the calculations happen in spherical coordinates, a memory block of the size ofthe image with height respective to the calculation parameters has to be allocatedfor every frame. This memory block can be cleared again after the results aretransferred to the cartesian map. However, depending on the parameters chosen,
several million data points would have to be allocated for each frame and cleared
after the calculation. As a consequence to that, the image has to be downsampled
to a smaller size.
Downsampling algorithms try to summarize the information given in many pixels
into one value. In this process, information is irrecoverably lost. Therefore, it isessential to find an algorithm, which preserves as much of the variance of the data as
possible. In the following, three downsampling algorithms will be examined and thehistograms of the images will be compared to the floored image histogram resulting
from the noise filtering above. The histograms presented are cut at the threshold 50
for more clarity. It is obvious that the floored image would have tens of thousands ofreturns at zero, which would make the graphic unreadable. The same is true for thehistograms of the downsampled images. There are significant numbers of returns in
the lower intensity ranges after the downsampling, however, the important features
are here assumed to lie above the threshold due to the flooring. An importantparameter for the downsampling is the downsampling factor. It represents the side
length of a square of pixels and thus, describes how many pixels will be summarized
into one. For the comparison ahead, a downsampling factor of four was used. This
results in 16 times lesser pixels.

4.2.1

Average Pooling

In average pooling, the average over all pixels in the scope will be calculated andset for the new value in the downsampled image. The resulting image is shown incomparison with the floored image (threshold 50) in figure 4.4. The effects are very
similar to average filtering. The image is blurred out and strong features are weakened. This behaviour becomes even clearer by the comparison of the histograms
presented in figure 4.5.

Figure 4.4: The floored image with threshold 50 on the left compared to the average
downsampled image with downsampling factor four on the right. The image isblurred out and strong features vanish.

31

4.2. Image Downsampling

Figure 4.5: The histogram of the average pooled image shows that high intensity
returns are flattened. All returns below 50 are cut for more clarity. Visual inspection
makes clear that the relevant variance of the data is not sustained.

4.2.2

Max Pooling

Similar as for average pooling, max pooling summarizes all pixels in the scope
defined by the downsampling factor into one value. It does so by choosing the maximum value given in the square. The resulting image is shown in figure 4.6. Again,
the effects are unpleasant. By choosing maximum values, fine contours, such as thesecond leg from the left, almost vanish. On the other side, remaining noise at higher
intensity values is strengthened. The histogram presented in figure 4.7 proves thathigh intensity values are over-present. However, the resulting histogram is a lot
closer to the original histogram than the average one.

Figure 4.6: The floored image with threshold 50 on the left compared to the max
pooled image with downsampling factor four on the right. The image looses details
and contours, while remaining noise is strengthened.

Figure 4.7: The histogram of the max pooled image shows that high intensity
returns are over-present. The underlying curve is flatter and data variance is lost.

Chapter 4. Image Preprocessing

4.2.3

32

Percentile Pooling

The weaknesses of the presented average and max pooling come from there tendency
to be extreme. Average pooling gives lot of weight to the zero returns (created aswell by the flooring process) and thus loses a lot of the high intensity returns. Max
pooling on the other side ignores all lower returns and loses contrast and details.
To counter these weaknesses, the last downsampling algorithm is more moderate
and looks for the 80-percentile of the pixels in the scope. For this, the pixels in thescope are ordered from lowest to highest value and the percentile is calculated. The
resulting value is high enough to take the strong responses into account, but low
enough to respect the low intensity returns. The resulting image is shown in figure
4.8.
The percentile pooled image is the closest to the original image. Although brighter,
it is less blurred than the others and preserves most of the details. Fine-tuning
the percentile factor might result in even better downsampling. The histogram
presented in 4.9 shows that the curves are indeed similar to each other but that alower percentile would likely perform better. After all, visual inspection proves thatthe percentile downsampling produces the most convincing results of the algorithms
presented here.

Figure 4.8: The floored image with threshold 50 on the left compared to the percentile pooled image with downsampling factor four on the right. The image isslightly brighter than the original, but preserves most of the structures.

Figure 4.9: The cut histogram of the percentile pooled image shows that most of
the variance is remaining. The most deviation is present at intensity values around
75.

33

4.2. Image DownsamplingChapter 5

Sensor Model
5.1

Introduction

The sensor model is the mathematical description and interpretation of a sensor’s
output. In a world, where a sensor would perfectly capture all information available and could give the exact data to the next client in the pipeline, a sensor model
would only have to transform the measured quantity into the desired one (for example time of flight into distance). Any interpretation of the data given would be
obsolete, because the data is fully representative of the state of the world. However,
due to limited resolutions, various noise and error sources, data quality degradationand data pre-processing, the sensor output does not show the world as it actually
looks like. The goal of the sensor model is to identify, and to compensate for, as
many relevant influences as possible, while still being computationally affordable.
This compensation is never perfect and predictions on the worlds state are not absolute, but are usually implemented in a probabilistic way. Probabilistic modellingassumes, that some disturbance factors (such as noise) can not, or not entirely, bemodelled. Therefore, these factors are treated as random variables and compensatedfor as good as possible according to their occurrence in the returns. In summary,the world is treated as deterministic, thus absolute, while the sensor responses are
regarded as probabilistic, thus not absolute.
In terms of occupancy, this means that if the sensor returns an occupied measurement of a section of the world, there is a certain probability that the measurement
and the place of measurement are true. In other words, a probabilistic sensor model
is somewhat similar to a lie detector, which states a certain probability for a sensor
statement to be true in the real world. By fusing several measurements, the model
becomes more and more certain about the truth.
In the general use of sensors, the final goal is thus to find the probability of theworld given the measurement. Or in a more intuitive way, how the world looks like
if only the sensor returns are available to reconstruct it. Depending on the sensor
and the return quality, this probability is rather high. The probability of objects
being present in the real world when found on RGB camera images is very high andeven further information on colour and state can be extracted with good certainty.
However, for less precise sensors, such as sonars, the likelihood of a correct measurement is significantly lower. As a result, the model requires more measurements
to become sure about the world. In any case, the probability of the world given
the measurement is usually very hard to define, as the world, even though real, is
not mathematically given. Bayes probability rule offers a workaround by deriving
the probability of the measurement given the world and by fusing this information
with the prior probabilities to find the desired quantity.
34

35

5.2. Elevation Information Recovery

In the following sections, the model to find the probability of a measurement given
the world will be derived step by step. First, a measurement has to be defined.
Different techniques will be presented, of which one will be selected. Then, therecovery of the missing elevation information will be examined, where again various
techniques will be discussed. And finally, a fusion framework will be built up that
includes the results of the previous steps and derives a formula for the probability
calculation.

5.1.1

Measurement Definition

When working with sensors on the basis of light, such as cameras and lidars, ameasurement is equal to a return. This means that if the sensor shows a reflection
at a certain point that this reflection is a measurement. For sonar, the definitionof a measurement can be a bit more complicated. Due to a low signal-to-noise
ratio created by crosstalk between the beams and external sources, a return is notnecessary a measurement.
In the image preprocessing chapter 4, the flooring operation was introduced toreduce the number of returns and filter as much noise as possible. The drawback ofthis method is though, that true, low intensity returns are disregarded. This mightlead to false negatives, thus occupied space labelled free. For the remaining returns,
there are two main approaches on how to use the values given.
The first approach is to treat all returns as equal measurements. By doing so, theexact value of the return is ignored and the calculation is performed the same forall returns. This is computationally the cheapest and overall the least complex
way to use the measurements. On the downside, probably valuable information isdisregarded. In 3.1, it was shown that a return is the summary of all reflections
given in a sphere segment. By ignoring the exact value, possible information on theamount of reflection in the sphere segment is lost.
This leads to a more complex approach. By using the received intensity value as
an indicator of likelihood of occupancy to the probability calculation, occupancy
could become more likely for higher values. The derivation of the bias, however, iscomplicated and relies on knowledge regarding the sensors’ data preprocessing anddisplay. Unfortunately, Blueprint Subsea stated upon request that the sonar does
indeed some sort of data preprocessing, which might even be different from beam
to beam. Therefore, this approach would be very labour intensive. As a result, thisapproach can be interesting for further improvement, but is not a valid option fora proof of concept.
In summary, it is difficult to define what a true measurement is in a sonar image.
For the scope of this work, the images will be floored to reject as much noise as
possible. After that, each return will be treated equally on a binary basis.

5.2

Elevation Information Recovery

In the sonar imaging process, the height information of objects, encoded in theelevation angle ϕ, is lost. Figure 5.1 shows illustrated the problem. As described in
section 3.1, an image pixel (bin) sums all reflections from the sphere segment given
ϕmax ϕmaxδrδθδθby (r, θ, ϕ) with r ∈ [rb − δr
2 , rb + 2 ], θ ∈ [θb − 2 , θb + 2 ] and ϕ ∈ [− 2 ,2 ].
Literature presents several ways to recover elevation information, yet none of themworks flawlessly in a general environment.

Chapter 5. Sensor Model

36

Figure 5.1: All reflections originating from an arc at fixed distance are collected
into one return in the image. The actual height of the stairs can not be extracted
from the image directly. [24]5.2.1

Acoustic Shading

The most common approach is to use acoustic shades to geometrically reconstruct
the height of objects [4] [17] [18]. While this works reasonably well for specificuse-cases, there are some hard constraints on the objects and the environment.
 The object lies on the seafloor, the shadow is connected to the object
 The shadow has to be clearly visible and lie completely inside the image
 Standard method does not work for staircase-like objects, as only highest
point throws a shadow
 For the geometrical derivation, the pose of the vehicle and the altitude above
seafloor have to be known

While this technique offers results with good precision for use-cases fulfilling therequirements, it adds further complexity to the model. Each frame has to be analysed for shadows and the geometric calculations have to be performed. But mostimportantly, the requirements for the algorithm to work are highly exclusive, whichmakes it not useful for general volumetric mapping.

5.2.2

Other Approaches

To generalize the acoustic shading approach described above, Negaharipour et al.
[18] extended their work and presented an algorithm that derives the backgroundplane normal vector n and uses it together with pose information to calculate theelevation angle of each point. This method still needs the assumption of a planar seafloor with small elevations, but is more flexible on the sea floor topography.
Guerneve and Petillot [13] showed a multi-view approach to work well for the elevation recovery of single objects. This gets rid of requirements on the seafloor or
the object position, but poses constraints on the motion and makes general mapping slow. DeBortoli et al. [15] introduced a convolutional neural network (CNN)
to extract elevation information directly from one image. However, besides of reasonably good results at short ranges, the algorithm is computationally very costly
and is not performable onboard. Additionally, the CNN is only trained on certain

37

5.3. Fusion Frameworktypes of objects, which again restricts the use-cases. Finally, McConnell et al. [29]introduced a technique using two forward-scan multi-beam sonars to reconstructarbitrary geometries, including elevation information. With little requirements towards the environment, the method can be widely used. However, it requires two
forward-scan sonars positioned orthogonal to each other, which does not only require
a lot of space and computation power on the robot, but is financially expensive.

5.2.3

Probabilistic Elevation Estimation

All algorithms presented have significant drawbacks, either on the requirements,
the computational costs or the possible use-cases. Therefore, a novel approach will
be presented here.
One way to avoid the issues described is by rejecting the idea of recovering the
precise height of an object. In occupancy mapping, it is important to know whichareas of the world are occupied and which are free. A precise reconstruction of
objects is therefore not necessary. So instead of geometrical calculations or precise
estimations, a probability density function PDF with mean on zero height andvariance half the vertical opening angle is introduced. Remembering the working
principle of the sonar described in 3.1, the return can come from any point or
multiple points in a sphere segment. By placing a PDF in the elevation direction,
the entire sphere segment becomes a probable source of reflection. This way, theelevation information is recovered implicitly, whenever a measurement is present.
Over several measurements, the occupancy probability of in fact occupied sections
should increase, while it should vanish for empty sections.

5.3

Fusion Framework

In this section, the information derived will be fused to derive a mathematical modelof the probability of the world given the measurement. Loop et al. [9] reformulated
equation 3.23 and included the measurement uncertainty along the range dimension.
In this work, their approach will be extended for angular uncertainty combined withthe elevation probability density introduced before. Therefore, the resulting framework takes uncertainty in all three axes into account. In the following, P denotes
discrete probabilities, while p stands for probability distributions.
The probability of occupancy Sx given the measurement Mr at a point x of the realworld is given asZ
P (Sx = 1 | Mr ) =P (Sx = 1 | r) · p(r | Mr ) dr

(5.1)

R3

Here, P (Sx = 1 | r) denotes the occupancy probability given the true position r
and p(r | Mr ) denotes the distribution of the true position given the measurement
Mr . The measurement offers, besides the response intensity, the angle and range
information from its position. For clarity, the measurement is here denoted with
Mr instead of z, due to possible confusions with coordinate axis. Expanding the
integral into the dimensions leads toChapter 5. Sensor Model38

Z π Z π Z ∞P (Sx = 1 | r, θ, ϕ) · p(r, θ, ϕ | Mr ) dr dθ dϕ
P (Sx = 1 | Mr ) =−π −π 0Z π Z π Z ∞P (Sx = 1 | r, θ, ϕ) · p(r | Mr ) · p(θ | Mr ) · p(ϕ | Mr ) dr dθ dϕ (5.2)
=
−π

−π

0

where the second equality comes from the independence of coordinate axis. In
the following, the occupancy probability given the true position is derived in two
different ways. The extended approach derived from Loop et al. [9] shown in 5.3.1
uses carving. This means that the occupancy probability of all points in front ofa surface (e.g. a response) is set to zero. Some researchers describe this method
as using the negative information, where negative indicates a wrong occupancy
hypothesis. In other words, this approach assumes that if there is no response, thespace must be free. While this method is usually valid for laser sensors, it can beproblematic for sonars due to multiple responses along a ray. Sonar signals have
low frequencies and can thus penetrate certain material. Additionally, the opening
angle of a beam, which is around 0.2° for the Blueprint Subsea M1200d [22], canallow multiple responses. Especially at larger distances, sound waves might passobjects and hit other ones further away, leading to multiple responses along one
beam. The adapted approach shown in 5.3.2 thus rejects the idea of carving andtreats areas in front of a surface as points with no additional information.

5.3.1

With Carving

The occupancy probability given the true position following Loops approach [9] isgiven by
0
P (Sx = 1 | r, θ, ϕ) = 1
12∥x∥ < r ∧ ∠xθ ∈ {θ ± ∆θ} ∧ ∠xϕ ∈ {ϕ ± ∆ϕ}r ≤ ∥x∥ ≤ r + τ ∧ ∠xθ ∈ {θ ± ∆θ} ∧ ∠xϕ ∈ {ϕ ± ∆ϕ}otherwise(5.3)

where r, θ, ϕ describe the true coordinates of the surface, ∥x∥, ∠xθ and ∠xϕ describe
the measured distance and angles of the point x, ∆θ and ∆ϕ the angular uncertainties and τ the thickness of the surface, equivalent to range uncertainty. While
it is possible to run the calculation on this representation of the function, it will beeasier to have a condition stating 0 probability in the otherwise case to split up thecalculation. This can be achieved by shifting the function by 21 . This results in

P (Sx = 1 | r, θ, ϕ) =− 1 ∥x∥ < r ∧ ∠xθ ∈ {θ ± ∆θ} ∧ ∠xϕ ∈ {ϕ ± ∆ϕ}1 1 2+ 2
r ≤ ∥x∥ ≤ r + τ ∧ ∠xθ ∈ {θ ± ∆θ} ∧ ∠xϕ ∈ {ϕ ± ∆ϕ}2 0otherwise

(5.4)

39

5.3. Fusion Framework

Now, the uncertainty function for the probability distribution of the position of apoint given the measurement p(r | Mr ) has to defined. Usually, this is realizedwith Gaussian distributions. Loop [9] showed however, that an issue arises from theinfinite support of the Gaussian distribution and the finite support of the surface
given by τ . They introduced a quadratic B-spline function Q, which is similar to a
Gaussian curve, but has limited support. It has to be mentioned that the original
B-spline function presented in the paper misses the normalization factor and thus
does not integrate to 1. Compensating for that, Q is defined as12

16 (3 + t)
1
1 8 (3 − t2 )Q(µ, σ) =1σ
(3 − t)2


 16
0

−3 ≤ t ≤ −1
−1 < t < 11≤t≤3
otherwise(5.5)

with t = σµ , where σ defines the variance of the quantity and µ stands for thedifference between true coordinate and measurement. The original approach provedthat the ideal choice τ is given by half the width of the B-spline function, thus 3σr .
Expanding this knowledge to the three dimensional case, the boundaries are givenbyτ = 3σr(5.6a)∆θ = 3σθ(5.6b)∆ϕ = 3σϕ(5.6c)

Using this, the distributions of the true position elements given the measurementarep(r | Mr ) = Q(r − ∥Mr ∥, σr )(5.7a)p(θ | Mr ) = Q(θ − ∠Mr , σθ )(5.7b)

p(ϕ | Mr ) = Q(ϕ, σϕ )(5.7c)

with σr , σθ and σϕ the variances of the distributions along the coordinate axes.
Putting this into equation 5.2 yields
Z π Z π Z ∞P (Sx = 1 | Mr ) =P (Sx = 1 | r, θ, ϕ) · Q(r − ∥Mr ∥, σr ) ·
−π

−π

0

· Q(θ − ∠Mr , σθ ) · Q(ϕ, σϕ ) dr dθ dϕ(5.8)

As Q(θ − ∠Mr , σθ ) and Q(ϕ, σϕ ) are not depending on r, they can be pulled outof the first integral
Z ∞Z π Z πQ(θ − ∠Mr , σθ ) · Q(ϕ, σϕ )P (Sx = 1 | Mr ) =−π−π· Q(r − ∥Mr ∥, σr ) dr dθ dϕ

P (Sx = 1 | r, θ, ϕ) ·
0

(5.9)

Chapter 5. Sensor Model40

At this stage, not only the integrands depend on the variables, but the boundaries
as well. As this results in problematic formulations, the boundaries of P (Sx =1 | r, θ, ϕ) given in equation 5.4 can be rearranged to

P (Sx = 1 | r, θ, ϕ) =0r < otherwise
1 1+ 2∥x∥ − τ ≤ r ≤ ∥x∥ ∧ θ ∈ {∠xθ ± ∆θ} ∧ ϕ ∈ {∠xϕ ± ∆ϕ}2 
 1− 2 ∥x∥ < r ∧ θ ∈ {∠xθ ± ∆θ} ∧ ϕ ∈ {∠xϕ ± ∆ϕ}(5.10)The given integrals in the range [−∞, ∞] can be split up in a part for points lying
outside the boundaries defined in otherwise and in a part where the conditions are
met. As a shortcut, the first part is given as constant 12 . This can be derived mathematically in the same way as the following, but can also be shown intuitively. By
definition, all sections of the world not covered in the conditions of P (Sx = 1 | r, θ, ϕ)
do not receive information from the measurement. Therefore, the occupancy probability of these points remains the same after the measurement. In log-odds space,
where the probability update happens, a probability of 21 results in a zero update.
Thus, the previous occupancy probability is not changed. This intuition proves
valid through calculation, which will not be derived here. However, the probability
for points lying in the boundaries given in P (Sx = 1 | r, θ, ϕ) has to be calculated.
For that, the range integral is split up and the boundaries of the angle integrals arechanged as

Z ∠xϕ +∆ϕ Z ∠xθ +∆θ
1
P (Sx = 1 | Mr ) = +Q(θ − ∠Mr , σθ ) · Q(ϕ, σϕ )·2∠xϕ −∆ϕ∠xθ −∆θ"Z∥x∥·
P (Sx = 1 | r, θ, ϕ) · Q(r − ∥Mr ∥, σr ) dr +
∥x∥−τ

Z ∞

#
P (Sx = 1 | r, θ, ϕ) · Q(r − ∥Mr ∥, σr ) dr dθ dϕ+∥x∥

Z ∠xϕ +∆ϕ Z ∠xθ +∆θ
1+Q(θ − ∠Mr , σθ ) · Q(ϕ, σϕ )·2∠xϕ −∆ϕ∠xθ −∆θ"Z∥x∥
1· Q(r − ∥Mr ∥, σr ) dr +·∥x∥−τ 2#Z ∞1+− · Q(r − ∥Mr ∥, σr ) dr dθ dϕ
2∥x∥
Z ∠xϕ +∆ϕ Z ∠xθ +∆θ1= +Q(θ − ∠Mr , σθ ) · Q(ϕ, σϕ ) ·2∠xϕ −∆ϕ∠xθ −∆θ"Z∥x∥
1·
Q(r − ∥Mr ∥, σr ) dr −2 ∥x∥−τ
#Z ∞−Q(r − ∥Mr ∥, σr ) dr dθ dϕ(5.11)=∥x∥41

5.3. Fusion Framework

The remaining integrals can be solved by substitution. Let u =and w = σϕϕ for more clarity. Thenθ−∠Mr−∥Mr ∥
, v = σθ r ,σru=r − ∥Mr ∥
du
1
→
−
=
→
− dr = σr · du
σr
dr
σr

(5.12a)v=

θ − ∠Mr
dv
1
→
−
=
→
− dθ = σθ · dv
σθ
dθ
σθ

(5.12b)

w=

dw
1
ϕ
→
−
=
→
− dϕ = σϕ · dv
σϕ
dϕ
σϕ

(5.12c)and the boundaries of the integrals change to


ϕ∈


θ∈

∠xϕ − ∆ϕ ∠xϕ + ∆ϕ,σϕ
σϕ

(5.13a)∠xθ − ∆θ − ∠Mr ∠xθ + ∆θ − ∠Mr
,
σθ
σθ


r1 ∈


r2 ∈



∥x∥ − τ − ∥Mr ∥ ∥x∥ − ∥Mr ∥,σrσr∥x∥ − ∥Mr ∥
, ∞
σr


(5.13b)(5.13c)(5.13d)Applying the substitution 5.12 and the adapted boundaries 5.13 into 5.11 yields

Z ∠xϕσ+∆ϕ Z ∠xθ +∆θ−∠Mrσθ
ϕ
1
P (Sx = 1 | Mr ) = + ∠x −∆ϕ ∠x −∆θ−∠M Q(v) · Q(w) ·
ϕ
θ
r
2
σϕ
σθ
"Z ∥x∥−∥Mr ∥
#
Z ∞
σr
1
·
Q(u)σr du − ∥x∥−∥M ∥ Q(u)σr du σθ dv σϕ dw∥x∥−τ −∥Mr ∥
r
2
σ
σ
r

r

(5.14)All variances cancel out in the multiplication with their corresponding Q function.
Let Q′ denote the presented B-spline function without the normalization factor σ1 ,thenChapter 5. Sensor Model42

Z ∠xϕσ+∆ϕ Z ∠xθ +∆θ−∠Mrσθ
ϕ
1
P (Sx = 1 | Mr ) = + ∠x −∆ϕ ∠x −∆θ−∠M Q′ (v) · Q′ (w) ·
ϕ
θ
r
2
σϕ
σθ
"Z ∥x∥−∥Mr ∥
#
Z ∞
σr
1
′
′
·
Q (u) du − ∥x∥−∥M ∥ Q (u) du dv dw∥x∥−τ −∥Mr ∥
r
2
σ
σ
r

r

(5.15)

Loop et al. [9] showed that the cumulative density of the quadratic B-spline measurement uncertainty function Q(µ, σ) is given by



0



1
3


 48 (3 + t)
1
1
Qcdf (µ, σ) = 2 + 24 t(3 + t)(3 − t)


1
3


1 − 48 (3 − t)1t < −3−3 ≤ t ≤ −1
−1 < t < 1
1≤t≤3
3<t

(5.16)At this stage, it is obvious that the remaining integrals are independent of eachother and can be solved separately. After reordering 5.15, the term looks likeZ ∠xϕσ+∆ϕ
Z ∠xθ +∆θ−∠Mrσθ
ϕ
1
P (Sx = 1 | Mr ) = + ∠x −∆ϕ Q′ (w) dw ∠x −∆θ−∠M Q′ (v) dv ·
ϕ
θ
r
2
σϕ
σθ
"Z ∥x∥−∥Mr ∥
#
Z ∞
σr
1
′
′
·
Q (u) du − ∥x∥−∥M ∥ Q (u) du∥x∥−τ −∥Mr ∥
r
2
σ
σ
r

Solving and plugging in the boundaries yields

r(5.17)

43

5.3. Fusion Framework

Z ∠xϕσ+∆ϕϕ∠xϕ −∆ϕσϕQ′ (w) dw = Qcdf (∆ϕ=3σϕ=Z ∠xθ +∆θ−∠Mrσθ∠xθ −∆θ−∠Mrσθ

=Z ∥x∥−∥Mr∥
σr∥x∥−τ −∥Mr ∥σr

Q′ (u) du = Qcdf (τ =3σr=Z ∞∥x∥−∥Mr ∥σr

Qcdf (m + 3) − Qcdf (m − 3)Q′ (v) dv = Qcdf (∆θ=3σθ

∠xϕ + ∆ϕ∠xϕ − ∆ϕ) − Qcdf ()σϕσϕ∠xθ + ∆θ − ∠Mr∠xθ − ∆θ − ∠Mr) − Qcdf ()σθσθ

Qcdf (n + 3) − Qcdf (n − 3)

Qcdf (o) − Qcdf (o − 3)

= (1 − Qcdf (o))∠xP (Sx = 1 | Mr ) =∠xθ −∠Mrσθ

(5.18b)∥x∥ − τ − ∥Mr ∥∥x∥ − ∥Mr ∥) − Qcdf ()σr
σr

Q′ (u) du = Qcdf (∞) − Qcdf (with m = σϕϕ , n =leads to(5.18a)and o =

(5.18c)∥x∥ − ∥Mr ∥)σr(5.18d)∥x∥−∥Mr ∥. Finally putting all terms togetherσr1+ (Qcdf (m + 3) − Qcdf (m − 3)) · (Qcdf (n + 3) − Qcdf (n − 3))·2
1
· ((Qcdf (o) − Qcdf (o − 3)) − (1 − Qcdf (o)))
21= + (Qcdf (m + 3) − Qcdf (m − 3)) · (Qcdf (n + 3) −2
1
1
− Qcdf (n − 3)) · ·(Qcdf (o) − Qcdf (o − 3) − )(5.19)
2
2

The lengthy derivation of the probability of the measurement given the world results in an elegant formula. Comparing to the result derived by Loop et al. [9],the formula has two more factors, one for the angular and one for the elevation
uncertainty. In summary, the occupancy probability given the measurement withcarving is given by

P (Sx = 1 | Mr ) =1+ (Qcdf (m + 3) − Qcdf (m − 3)) · (Qcdf (n + 3) − Qcdf (n − 3))·2
1
1
· Qcdf (o) − Qcdf (o − 3) −2
2
(5.20)

Chapter 5. Sensor Model

5.3.2

44

Without Carving

The process and the calculations are very similar to the carving case. Only thechanging parts will be presented again. The main difference occurs in the occupancy probability given the true position, which is no longer zero for points in frontof the measurement, but 12 as well. This corresponds to the statement that no new
information was received. Additionally, the surface thickness estimate is doubled,with the assumption that the true surface centre is at the point of the measurement.

P (Sx = 1 | r, θ, ϕ) =(1r ∈ {∥x∥ ± τ } ∧ θ ∈ {∠xθ ± ∆θ} ∧ ϕ ∈ {∠xϕ ± ∆ϕ}1+ 2
20 otherwise(5.21)

Using the same B-spline model for the measurement Q as defined in 5.5 and assuming again τ = 3σr , ∆θ = 3σθ and ∆ϕ = 3σϕ , the calculation of the occupancy
probability given the measurement isZZ ∠xθ +∆θ1 1 ∠xϕ +∆ϕ
P (Sx = 1 | Mr ) = +Q(ϕ, σϕ ) dϕQ(θ − ∠Mr , σθ ) dθ·
2 2 ∠xϕ −∆ϕ∠xθ −∆θZ ∥x∥+τ·Q(r − ∥Mr ∥, σr ) dr
(5.22)∥x∥−τ

Applying again the substitution from 5.12, changing the boundaries as defined in
5.13 and multiplying the variances into the B-spline functions yields

1 1
P (Sx = 1 | Mr ) = +2 2Z ∠xϕσ+∆ϕϕ∠xϕ −∆ϕσϕ

Z ∥x∥+τσ−∥Mr ∥·r∥x∥−τ −∥Mr ∥σr

Z ∠xθ +∆θ−∠Mrσ′

Q (w) dwθ∠xθ −∆θ−∠Mrσθ

Q′ (v) dv ·Q′ (u) du(5.23)

With the definition of cumulated density function Qcdf as in 5.16, the result is

P (Sx = 1 | Mr ) =1 1+ (Qcdf (m + 3) − Qcdf (m − 3)) · (Qcdf (n + 3) − Qcdf (n − 3))·2 2
· (Qcdf (o + 3) − Qcdf (o − 3))1= (1 + (Qcdf (m + 3) − Qcdf (m − 3)) · (Qcdf (n + 3)−
2− Qcdf (n − 3)) · (Qcdf (o + 3) − Qcdf (o − 3)))(5.24)∠x∠x −∠M∥x∥−∥M ∥θrr
Same as for the carving case, m = σϕϕ , n =and o =
. It isσθσrintuitive that the main difference between the two formulas occurs in the range

45

5.4. Sensor Model Validation

term, as the others are not affected by the changes. The formula shows that now
the range term has the same shape as the azimuth and elevation angle terms. In
summary, the formula for the occupancy probability given the measurement withoutcarving is given by

P (Sx = 1 | Mr ) =1(1 + (Qcdf (m + 3) − Qcdf (m − 3)) · (Qcdf (n + 3) − Qcdf (n − 3))·2
· (Qcdf (o + 3) − Qcdf (o − 3)))(5.25)

5.4

Sensor Model Validation

In the upcoming analysis, both the carving and the non-carving case will be examined. By the end of this section, the more suitable of the two will be derived.
In a first step, the formulas derived in the sensor model section 5 will be implemented for a single point. This neglects all relation to sonar imagery, but should
just demonstrate that the formulas produce reasonable results. Continuing, themodels will be shown for simulated sonar imagery with two point responses. Thisway, the behaviour of the framework for returns with overlapping support will beshown. And finally, the real sonar image of a folding chair on the lake floor will beprocessed.

5.4.1

Point Response

Figure 5.2 shows the results for the carving model for a single point response inspherical coordinates in probability space. The left image shows the behaviour ofthe framework at zero elevation. As expected, the area in front of the point is set tozero occupancy probability due to the carving. According to the boundaries set inequation 5.4, a surface thickness τ occurs behind the point, stating a high occupancy
probability there. The right image shows the same framework with results off centre,
thus at non-zero elevation. The blurring along the elevation axis is clearly visible,
as the probabilities off centre are lower than the ones at zero elevation.
The figure shows that the carving framework produces results as expected for asingle point response.

Figure 5.2: The results of the carving model plotted for a single point response.
On the left, the results are shown at zero elevation, on the right off centre. The
probability blurring along the elevation axis is clearly visible.

Chapter 5. Sensor Model

46

Figure 5.3: The results of the model without carving plotted for a single point
response. On the left, the results are shown at zero elevation, on the right off
centre. The probability blurring along the elevation axis is clearly visible.
Figure 5.3 shows the results for model without carving in probability space. Similar
as for the carving model, the left image shows the probability distribution at zeroelevation, while the right image shows it off centre. Compared to the carving model,
the area in front of the point is treated the same way as all other areas. According
to equation 5.21, the thickness τ of the surface is now centred at the response point.
This takes into account that range uncertainty is present in front and behind thepoint.

5.4.2

Two Point Response

In this section, the two models will be used to calculate the occupancy probabilities for a simulated sonar image containing two point responses, placed at (171,220) and (342, 220), respectively. The coordinates correspond to vertically centred
points on the first and second third mark along the horizontal. This test should
show the behaviour of the models for overlapping responses. For that, the variancesin the models were chosen such that an overlap happens. Additionally, the scale
is changed from probability space to log-odds space to easily combine the results.
The reader is reminded that the log-odds space is defined in the range [−∞, ∞].
For clarity, the results are only shown at zero elevation, as the elevation blurring
has already been illustrated before.
Figure 5.4 shows the performance of the models applied on the two point response
image. The scale shows that the models become more certain about free and occupied space where the supports of the responses overlap. It has to be mentioned
that, in log-odds space, the carving model creates negative values due to its usageof negative information. On the other hand, the model without carving never falls
bellow zero, thus 21 in probability space. This is intuitive, as no negative information
is used that could push the probability below the prior.

47

5.4. Sensor Model Validation

Figure 5.4: The colour maps illustrate the overlapping support functions for thecarving model (left) and the non-carving model (right).

5.4.3

Folding Chair

Two Dimensions
Now, the models will be tested on a real sonar image showing a folding chair on
the lake floor, see figure 5.5. It is the same floored image derived in the image
preprocessing 4.

Figure 5.5: The floored image of a folding chair on the lake floor as an example forreal sonar imagery.
Figure 5.6 shows the results of the models applied to the chair image in two dimensions. The left image displays the output of the carving model. By visual inspection,
it becomes clear that the model is very sure about free space, but has problem toidentify occupied space. Compared to figure 5.5, the chair is almost gone. This ismainly due to the carving influence of the lake floor responses.
On the right side, the results from the model without carving is shown. As explained
before, the model does not produce values bellow zero due its lack of negative information usage. However, the model becomes very sure about occupied space andalso identifies free space accordingly. This is a very promising result, as the level ofdetail is relatively high.

Chapter 5. Sensor Model48

Figure 5.6: On the left, the result of carving model applied to the folding chair
image. The model struggles to maintain occupancy information. On the right, thesame for the model without carving, showing much better results.
Three Dimensions
In this last step, a three dimensional plot of the results will be presented. As shownin the previous section, the model without carving produces better results than thecarving model. Therefore, only the non-carving model will be shown here in thethree dimensional environment. Figure 5.7 shows a three dimensional rendering of
the results. The elevation variance was chosen much larger than the angular and therange variance to show its effects. Each feedback is multiplied with a Gaussian-like
quadratic B-spline function to adjust for elevation uncertainty. Figure 5.8 shows a
top view of the same rendering, making clear that the underlying two dimensional
image is still visible.

Figure 5.7: A three dimensional rendering of the results of the probability calculation based on the chair image from the sonar point of view.
In summary, the sensor models derived in chapter 5 are valid and produce theexpected results. It was shown that the model without carving produces performs
better, as it is more certain about occupancy.

49

5.5. Probability Scaling

Figure 5.8: The top view on the three dimensional rendering of the probability
calculation shows detected returns. The chair and its legs are clearly visible.

5.5

Probability Scaling

The sensor model derived and validated in this chapter fuses uncertainties fromvarious sources, such as the pose estimator and the elevation information recovery.
Therefore, the model states how certain we are about the position of the returnby summarizing the effects of named uncertainties. However, the framework does
not cover the uncertainty if a return is actually present at all. An elegant way toachieve this is by limiting the impact a single measurement can have. A solution tothat is probability scaling, where the range of possible probabilities in the update
is restricted.
In practice, the calculated occupancy probability is scaled from the range [0, 1] toa smaller range [ 12 − α, 12 + α], with α ∈ [0, 21 ]. By observation, the adapted range
looks very similar to the derived sensor model P (Sx = 1 | Mr ). In fact, the firstsummand indicates the centre at 12 , while the second summand in both formulas
take values in the range [− 12 , 21 ]. By pre-multiplying the second summand with ascaling factor β ∈ [0, 1], the scaling can be achieved very easily. This results in thefollowing updated formulas for the case with carving

1
Pscaled (Sx = 1 | Mr ) = + β(Qcdf (m + 3) − Qcdf (m − 3)) · (Qcdf (n + 3)−
2
1
1(5.26)− Qcdf (n − 3)) · (Qcdf (o) − Qcdf (o − 3) − )22

Chapter 5. Sensor Model50

and for the case without carving

1
Pscaled (Sx = 1 | Mr ) = · (1 + β(Qcdf (m + 3) − Qcdf (m − 3)) · (Qcdf (n + 3)−
2− Qcdf (n − 3)) · (Qcdf (o + 3) − Qcdf (o − 3)))(5.27)

Intuitively, β = 1 corresponds to the original, unscaled state and β = 0 to a constant occupancy probability of 21 with no new information. A suitable value for β
has to be found empirically. A reasonable initial guess is β = 0.5. The range ofprobabilities in probability space is then [0.25, 0.75]. In other words, a single return
receives at maximum 75% impact capability compared to the unscaled case. The
update in log-odds space is thus bounded in the range
 
ln


1
1
2 − 0.5 · 2,
1 − ( 12 − 0.5 · 12 )


ln


1
1
2 + 0.5 · 2≈ [−1.1,
1 − ( 12 + 0.5 · 12 )1.1]

(5.28)

This adapted range of the update in log-odds means in practice that, for every
return, the occupancy probability of a point in log odds-space can maximally be
reduced or increased by 1.1.

51

5.5. Probability ScalingChapter 6

Occupancy Map
In this chapter, the practical implementation of an occupancy map on the basis of
the derived sensor model will be presented. In practice, there are several aspects tobe resolved, such as the unbounded log-odds space, the definition of an occupancy
threshold and free space compensation. In the following sections, approaches forthese problems will be discussed.

6.1

Clamping

In 3.2.2, the log-odds transform was introduced to move the probability updatein a space, where the updates could just be summed. This transform has many
advantageous, but comes with one significant drawback, namely, that the space isunbounded. This means that the log of occupancy probability of a point can growto positive or negative infinity with probabilities close to 0 and 1 or with many
accumulated observations. In 5.5, the probability scaling was introduced to limit
the update a single return can create. Even though this has as well significant impact
on the speed of growth of the probability in log-odds space, the property remains
unbounded. The main reasons to avoid this infinite growth are the memory costfor storing and handling very large numbers and the limited use of high precisionof occupancy probabilities close to one.
Clamping is a common approach to limit the growth of the probabilities in log-odds
space. Applying clamping means to set a high ωhigh and a low threshold ωlow ,above and below which no further update is performed. If an update would overor undershoot the threshold, the clamping thresholds would be imposed. Typicalvalues are ωhigh = 0.99 and ωlow = 0.01. In log-odds space, these values become±4.59. In theory, the thresholds do not need to be symmetric, but it makes sense forthe present case. Setting the clamping thresholds is one of the tuning parametersof the model, as the thresholds define how much ”counter-evidence” is needed forthe conclusion to change. Taking the example scaling factor of β = 0.5 given in 5.5,
the maximum update per return is ±1.1. This means that it takes approximately 4
individual returns to remove the evidence for a conclusion. In reality, this number if
a lot smaller due to overlapping return supports. In summary, clamping limits therange of probabilities in log-odds space and tuning the thresholds is the set screw
to how fast the model changes its conclusions.

52

53

6.2

6.2. Serialization

Serialization

This section focuses on the reduction of computational cost by serializing the sensor
model given by the formulas 5.20 and 5.25. In two dimensions, the output of theformulas applied on all returns of an input image is a probability matrix of thesame size as the image, called base matrix. An original image thus results in a 440
x 512 matrix, while the downsampled image in a 110 x 128 base matrix. For thethree dimensional case, the number of elevation levels is a set screw to tune. The
output of the formulas result in a three dimensional matrix, here called probability
tensor, built from stacked layers of probability matrices. For a downsampled image
and eleven elevation levels for example, the tensor is of shape 110 x 128 x 11. Thisis a rather high number of data points to be calculated and revisited. In practice,as few as possible calculations should be performed, where serialization comes into
play.
Visual inspection shows that the formulas consist mainly of three factors of similar shape and a constant offset of 0.5. Each factor is independent in its direction,
meaning that one contributes the range, one the angular and one the elevation uncertainty. The independence of these factors allow to serialize the calculation ofthese models.
In theory, all three dimensions can be calculated separately and fused in a final
calculation step. However, in the zero elevation plane, the support of the models of
two neighbouring returns overlap, which makes the serialization less effective. Still,some points have to be revisited several times for multiple overlapping supports.
Additionally, the definition of the range and azimuth angle variances σr and σθallow to limit the support area significantly. Low variances already result in lower
computational cost.
However, the elevation dimension offers great potential for cost saving. In this direction, no overlapping supports occur, as there are no returns along this axis. This
results in a very practical property, namely, that the elevation factor is constant for
all points at a certain elevation level, therefore for a horizontal slice in the probability tensor. Using this knowledge, the probability matrix at zero elevation (image
plane) can be calculated independently of the elevation factor. Only later, this basematrix is multiplied with the corresponding factors to create elevation levels. In
a more graphical understanding, applying a gaussian-like uncertainty function to
every single point gives the same result as applying one gaussian-like uncertainty
function to the entire zero elevation plane. In the following, the no-carving formulawith the scaling factor 5.28 will be serialized according to the derivations. The
procedure is then similar for the carving case.
The original formula is given by

P (Sx = 1 | Mr ) =1 1+ β(Qcdf (m + 3) − Qcdf (m − 3)) · (Qcdf (n + 3)−
2 2− Qcdf (n − 3)) · (Qcdf (o + 3) − Qcdf (o − 3))(6.1)

Let f stand for the range factor Qcdf (o + 3) − Qcdf (o − 3) and g and h for azimuthand elevation angle factors, respectively. The formula then reads as

P (Sx = 1 | Mr ) =1 1+ ·β·f ·g·h
2 2

(6.2)

Chapter 6. Occupancy Map

54

For the zero elevation plane, the elevation factor has no impact, therefore

ϕ=0

P 0 (Sx = 1 | Mr ) =1 1+ ·β·f ·g
2 2

(6.3)

The elevation factor h can now be evaluated for discrete elevation levels. To find theprobability matrix at a certain elevation level, the elevation factor can be multiplied
to the base matrix P 0 . However, a compensation factor λ has to be introduced tocompensate for the multiplication of h with the summand 12 as

P ϕ (Sx = 1 | Mr ) = P 0 (Sx = 1 | Mr ) · h(ϕ) + λ(6.4)

By requiring the same results for the original and the serialized case, λ can easily
be derived asλ=1(1 − h(ϕ))
2

(6.5)

Plugging this into 6.4 leads to1
P ϕ (Sx = 1 | Mr ) = P 0 (Sx = 1 | Mr ) · h(ϕ) + (1 − h(ϕ))
2

(6.6)

Back substitution shows that 6.6 and 6.1 are indeed equivalent for a fixed elevationlevel. This derivation shows that the probability tensor can be calculated by first
deriving the zero elevation base matrix and then adding the elevation components.
The effects on the computational cost can be estimated by the number of function
evaluations that have to be performed. For the regular case, all three factors haveto be evaluated for every entry in the probability tensor. Therefore, each entry
requires six evaluations of the Qcdf function. Assuming for example a low numberof 11 elevation steps and a downsampled image of size 110 x 128 pixels, this leads toover 84’400 evaluations for the zero elevation layer and over 929’000 evaluations forthe entire tensor. In comparison, the serialized case only requires four evaluationsof Qcdf per pixel in the base plane and in total 11 · 2 = 22 evaluations for theelevation factors, two evaluations per elevation level. This results in little over
56’300 evaluations for the entire probability tensor. This corresponds to a reductionof almost 94% of Qcdf evaluations compared to the standard approach. It has tobe mentioned though that additional multiplications have to be performed for theserialized case. Their influence, however, is negligible compared to the impact of
Qcdf evaluations.

55

6.3

6.3. Interpolation

Interpolation

In 3.2.1, it was shown that the occupancy map is built from voxels defined in cartesian space. Voxels are equally sized cubes that contain their position information
and additional attributes. The sensor model, however, works on the image points
and is therefore based on spherical coordinates. Assuming constant angle and range
increments, the volumetric elements in spherical space are not equally sized. Spheresegments close to the sensor are very small, whereas elements at outer ranges become very large due to growing arc segment lengths. As a result, the probability
that is derived by the sensor model for individual image points has to be transferredto the cartesian map space.
In a first step, the affected voxels in cartesian space have to be identified. Here, thedefinition of the field of view (FOV) given in 3.1.3 comes into play. Using the coordinate transforms given in 3.1.2, the current sensor pose in cartesian coordinates
can be derived. Applying standard coordinate frame transforms, the field of viewof the sensor in the global cartesian frame is obtained. From the perspective of
the mapping algorithm, this allows for a simple check to determine affected voxels.
By iterating through the map, voxels in the current FOV are labelled for further
update, while all others are disregarded.
In a next step, the data points in spherical coordinates relevant for an individual
voxel have to be identified. For that, the centre of a previously for update labelled
voxel is mapped to spherical coordinates and the closest neighbours are derived.
Upon knowing all relevant data points for the update, the interpolation process canbe started. Kang illustrated the technique of trilinear interpolation to extract information in three dimensions from an external frame [30]. Figure 6.1 shows the basic
setup assuming an equally spaced cartesian grid. The method performs seven linear
interpolation steps to find the desired quantity at point p. The lengthy formulas will
not be presented here, however, they can be easily derived by performing the nested
interpolations. The method, originally introduced for colour values in volumetricgrids, can as well be implemented for the derivation of probability values. Using
the closest neighbours derived by the last step, the interpolation method, for theremainder of the chapter indicated by T (), can be applied to find the probability
update value.

6.4

Free Space Compensation

In 5.4, it was shown that the no-carving model performs generally better. However,it comes with the major drawback that no occupancy probabilities bellow 0.5 canbe generated due to it ignoring negative information. As a result, noise and weaksignals can leave traces in the map, as no handle exists to remove them. This behaviour is obviously not desired because the noisy imagery of the sonar might leadto many accumulated artefacts.
To counter this, free space compensation is introduced. The basic idea is to reduce
the occupancy probability of all points in the current field of view by a small value.
Points, that are truly occupied, will be merely affected by the reduction, as thevalues created by signals are significantly larger. But over time, truly free space
will be labelled as such, when many observations are accumulated. By the working
principle of this method, negative information is implicitly used. Let Pocc denotethe occupancy probability of a voxel in log-odds space after the interpolated updateand ∆P the compensation factor. Then, the final occupancy probability after one
update step, thus after one image, is given byChapter 6. Occupancy Map

56

Figure 6.1: The trilinear interpolation method presented by Kang [30] uses sevenlinear interpolation steps to derive the value at the desired location p.

Pocc,new = Pocc − ∆P

6.5

(6.7)

Update Algorithm

This section summarizes all derivations made in the previous chapters and presents a
pseudo-code for the update algorithm in 1. On a basic level, the algorithm consists ofThree main parts. First, the preprocessed input image is used to derive a probability
tensor, as shown in lines 5-9. This tensor contains the overlapping supports of thesensor model evaluated for all returns in the image. For every new image, thistensor is calculated anew. Second, all voxels in the map are reviewed and sorted
according to their position in the field of view. All voxels, which are not visible,
do not get an update and the algorithm continues. The last part is the update of
the voxels in the current view. For that, the interpolation method is called on thecorresponding position of the voxel in the probability tensor. The interpolation of
the occupancy probability is shown in line 11. Finally, line 12 shows the free spacecompensation, where the occupancy probability of each voxel in the current field ofview is reduced by a constant factor.
The algorithm is called for every incoming sonar frame. One can imagine thatthe computational cost for the calculation of the probability tensor and for theinterpolation are rather high, which makes them key contributors to computation
time. The influence of the voxel reviewing in line 10 would have to be examinedfurther, if timing becomes critical.

57

6.6. Occupancy Threshold

Algorithm 1 Voxel Update Algorithm
1: I = Preprocessed Input Image
2: PTensor = Probability Tensor
3: SM () = Sensor Model
4: T () = Interpolation Function
5: for Pixel in Image do
6:
if Ir,θ > 0 then7:
PTensor ← SM (Ir,θ )8:
end if
9: end for
10: for Voxels in FOV do
11:p(Sx ) = T (PTensor (r, θ, ϕ))12:p(Sx ) = p(Sx ) − ∆P
13: end for

6.6

Occupancy Threshold

The formulas derived in the sensor model section 5 provide a probability of occupancy. However, this value has to be interpreted to build a binary map withoccupied and free voxels. To build a map from the calculated values, a thresholdϵ ∈ [0, 1] in probability space is introduced. This threshold is the set screw to tune
the model. The higher the threshold, the lower is the false positive rate, meaning,the more sure the model is about occupancy. On the other side, the lower thethreshold, the lower is the false negative rate. In other words, the model is moresure about free space. Additionally, a low threshold is prone to noise error andintroduces artefacts, whereas a high threshold tends to neglect true occupancy andthus missing objects. It is therefore important to tune the threshold as good as
possible for a given use-case, which is more than often an empirical trial and error
process.
For the area of occupancy mapping for navigation and localization tasks, false negatives, thus occupied space labelled free, is very dangerous. Collisions with objects
or the sea floor can not only cause damage to the platform, but can also disturb
the state estimation process significantly. As a result, it is reasonable to assume arather low threshold to be suitable. After the definition, the threshold given in theprobability space has to be transformed to the log-odds space.

In occupancy space: ϵIn scaled space: ϵscaled =(6.8a)11+ β(ϵ − )2
2

(6.8b)

ϵscaled)1 − ϵscaled(6.8c)

In log-odds space: ϵlog = ln(

Using the scaling value β = 0.5 and a threshold ϵ = 0.75 in occupancy space as an
example leads to an ϵscaled = 0.625 in scaled space and thus ϵlog = 0.511 in log-odds
space. Every point with a value above this threshold is considered to be occupied,
everything below to be free.

Chapter 6. Occupancy Map

58

Chapter 7

Integration in Voxblox
After the model validation shown in 5.4 and the practical aspects for occupancy
mapping presented in 6, the final step is the integration of the novel approach
into Voxblox. Voxblox offers tools for map update and visualization, which allow
to further test and analyse the behaviour of the model. For this work, the scope
stays in the implementation, while testing and visualization will be open tasks.
In 2.4, Voxblox was introduced on a basic level as a mapping and path planninglibrary. It is built for the usage with point clouds created from depth cameras or
Lidars and performs very well in comparison to other state of the art methods. It
was shown, however, that the current implementation of Voxblox has difficulties
to handle uncertainty, which makes it hard to use for underwater robotics. But
as a flexible volumetric voxel-based system, it can basically represent any three
dimensional field with independent cells. Therefore, with some changes, it canrepresent occupancy maps too. For the integration, the general structure of theoriginal Voxblox will be first examined in more detail, before the adaptations needed
will be shown. The upcoming analysis does not claim to be complete, but rather
focuses on the parts relevant for the project.

7.1

Structure

7.1.1

Old Structure

In the following structure analysis, Voxblox will be broken down top to bottom, froma very general description to the explanation of individual modules. As a starting
point, Voxblox takes the sensor output as input and performs calculations on it.
The results of these calculations are then used to build, update and maintain a mapthat enables, among others, path planning. Voxblox can be used as a stand-alone
library, but more often, it is used in a Robot Operating System ROS network. A
dedicated package exists that handles all communication over ROS. For underwater
robotics, where an entire system has to communicate, the implementation with
ROS is unavoidable. Figure 7.1 shows the general data flow of Voxblox in a bit
more detail.
Voxblox is built for the usage with pointclouds, which often originate from depth
cameras or Lidars. Pointclouds are three dimensional return arrays that collect
returns created on the basis of the time-of-flight principle. As explained in 2.4,
Voxblox creates Truncated Signed Distance Fields (TSDFs) to reconstruct surfaces.
The TSDF-Server is one of the key modules of Voxblox and handles all communication regarding TSDF information and incoming sensor messages. It can beunderstood as some sort of post office that collects the published data from thesensor, prepares it and hands it over to the TSDF-Integrator, where the TSDF map
59

Chapter 7. Integration in Voxblox

60

Figure 7.1: The original structure of Voxblox illustrated on the simplified data flowfrom sensor output to visualization.

building happens. As the name suggests, the TSDF-Integrator uses the information
extracted from the pointclouds and fuses it into the map. On a side note, it can be
mentioned that the current implementation of Voxblox uses ray-tracing, therefore
a method to make use of negative information (carving). For each return, a ray isdrawn from the origin (sensor position) to the respective point. All voxels along the
ray are labelled as free and excluded from further calculations. The voxels affectedby the return are updated according to the results of the TSDF calculation. Uponfinishing the map update, the integrator returns to the TSDF-Server. In theory,
the TSDF map can already be used as it is an implicit surface representation. In
Voxblox, the TSDF map is used to build Euclidean Signed Distance Fields (ESDFs),
with which path planning can easily be performed. The visualisation modules arebuilt such that they can publish various properties, such as slices, the TSDF or
ESDF maps and surface reconstructions.

7.1.2

New Structure

As a part of this work, the integration of occupancy mapping into Voxblox can beachieved by creating new modules replacing the TSDF-related elements in the oldstructure. As a matter of fact, the occupancy elements can be implemented on theside as a new module, without changing much to the original structure of Voxblox.
A basic occupancy integrator with pointcloud input is already implemented, but is
not fully connected to the rest of Voxblox. For the main part, the remaining changes
focus on the TSDF-Server and the TSDF-Integrator, which will be replaced by a
new occupancy server and integrator. Figure 7.2 shows the adapted data flow.

61

7.1. Structure

Figure 7.2: The updated structure of Voxblox used for occupancy mapping illustrated on the simplified data flow from sensor output to visualization. Main changes
occur for the input server and the integrator.

The sensor output is no longer a pointcloud, but an intensity image, which canbe handled as a 2D grey-scale image. As before, the occupancy server manages
all communication with ROS. It gets the images from the sensor and puts theminto a queue. Furthermore, the image preprocessing described in 4 is performedin the occupancy server and has to be implemented accordingly. This allows toclearly separate the actual image handling from the probability calculation andmap update. In summary, the entire pipeline of sensor output message handlinghas to be adapted to now work with 2D imagery instead of pointclouds.
After the image preprocessing, the server hands the floored and compressed image
over to the integrator. The general functionalities here are the same as describedin the pseudo-code 1. In comparison to the original Voxblox implementation, acompletely new integrator has be to implemented. The occupancy integrator is thepart containing the derived new sensor model from 5. It calculates the probability
tensor from the preprocessed image, looks for affected voxels and performs the
update on the map. This as well includes the trilinear interpolation described in
6.3. Compared to the original Voxblox implementation, a lot more calculations are
now performed. It is very likely that this leads to higher computation times, but
this would have to be tested in a one on one comparison of an identical 3D volume.
From there on, the data flow is very similar to the one in the old structure. The
occupancy server publishes the updated maps and provides them to the ESDFServer and the visualisation modules. The latter two only need minor changes towork with the provided occupancy maps, because ESDF-maps can be built fromoccupancy maps as well as from TSDF maps.

Chapter 7. Integration in Voxblox

7.2

62

Testing

For the scope of this work, the testing is rather rudimentary. After showing that thederived sensor models are valid in 5.4, the testing in Voxblox focuses on the correct
implementation. To achieve that, a sonar simulator created at ETH Zurich is used
to create sample images. With the simulator, it is possible to create subsequent
images with known poses to recreate a real motion scenario. Figure 7.3 shows
an example image displaying a cubic object and a cage on an uneven, sandy floor.
Visual comparison makes clear that the imagery from the simulator has only limited
meaningfulness, as the floor creates a lot more high intensity returns than the one
in the chair image used earlier. Additionally, the omnipresent crosstalk noise in theoriginal image and the range-dependent image degradation are missing. Overall,
the simulator imagery is useful to test the general implementation in Voxblox, as
it can be considered an easier case than the real one with the missing noise. But
proper testing would in a next step require true sonar data.

Figure 7.3: An example image from the sonar simulator. Missing noise and image
degradation and the overly present high returns of the floor result in low overall
meaningfulness of the simulator imagery. However, it can be considered an easy
test case to validate the occupancy implementation in Voxblox.
The testing shows that the implemented occupancy server is working. The sonar
simulator image is published together with the sonar pose from a separate script to
replicate a true sonar publisher. The incoming images are put in a queue, whichworks on the first in first out (FIFO) principle. The current image is then extracted
from the queue and the coordinate frame transforms between world frame and sensor frame are performed. In a next step, the image preprocessing happens, where
the image is floored and compressed. Last but not least, the image is handed overto the integrator successfully. This corresponds to the desired work flow described
previously in 7.1.2.
The occupancy integrator receives the image from the server and aims to update themap accordingly. The integrator as the newly designed core part of the occupancy

63

7.2. Testing

implementation required special attention to work properly with the remaining characteristics from Voxblox. Therefore, the interfaces to the occupancy server and tothe map representation, thus the voxel handling, are crucial.
Voxblox uses a specialized indexing system, where voxels are grouped into blocksof 16. Additionally, Voxblox builds the map as it grows, thus, unvisited areas are
not yet initialized. As a result, first the affected blocks have to be identified andthe memory capacity allocated. For the current implementation, this is done witha measurement independent bounding box. In the future, a method to dynamicallygrow the map based on the current measurement will reduce the allocated memory
by the unnecessary parts. All blocks inside that bounding box are revisited. In
each block, the voxels indices are extracted and projected into the sensor frame.
By doing so, it can easily be checked if a voxel is in the current FOV. If the checkis passed, the trilinear interpolation function calculates the interpolated value fromthe probability tensor using the projected indices. As the final step, the interpolated probability is written to the voxel properties.
Figure 7.4 shows a three dimensional rendering of the result. The input stream is asequence of images created from the sonar simulator. The pointcloud uses intensity
values corresponding to the occupancy probabilities and encodes them in a colour
scheme. In the same rendering, occupied voxels are displayed, again with a colour
code. All displayed voxels exceed the occupancy threshold set for this example. It
has to be mentioned that the parameters of the model are not specifically tuned
for the case given. Therefore, the results proof the continuous update, the freespace compensation and the underlying concepts valid, without giving information
about the map quality. Figure 7.5 shows then a slice at a fixed z-coordinate of thepointcloud. The colour scheme again indicate the occupancy probabilities calculates by the model. In summary, the aforementioned interfaces between server andintegrator and between integrator and voxel handling work as intended and makethe newly implemented modules valid extensions to Voxblox.

Figure 7.4: A 3D rendering of the model output on the basis of an image sequencecreated in the simulator. The pointcloud displays the occupancy probabilities at
the grid points, whereas the boxes display the voxels that exceed the occupancy
threshold.

Chapter 7. Integration in Voxblox

64

Figure 7.5: A slice at a fixed height z through the pointcloud shown in 7.4. The
colour scheme indicates the calculated occupancy probabilities.

65

7.2. TestingChapter 8

Conclusion
8.1

Summary

Mapping is one of the key contributors to successful autonomous mobile robotics.
Particularly navigation, but as well object localization, recognition and manipulation depend on precise and computationally affordable mapping. While for aerial
and land robotics techniques based on depth cameras and Lidars are predominant,sonars are the preferred sensor choice for underwater robotics. Being more reliablein the difficult environments of turbid water with low illumination levels, they overcome the limitations of light-based sensors. However, forward-scan sonars comewith an own set of challenges, including noisy imagery and the loss of elevation
information.
To counter the overly present noise in the sonar images, a representative analysisof the intensity values on an example image of a folding chair on the lake floor was
performed. It showed that many returns in the image have low intensity values andmight thus originate from noise. A valid solution was found in the flooring operation, which had similar effects to a contrast enhancement. To reduce the numberof pixels, on which the calculations would have to be performed, a percentile compression was introduced that preserved the variance of the data, thus the intensity
distribution.
Leaving the image preprocessing, a novel sensor model was introduced that included
a new approach to tackle the loss of elevation information in forward-looking sonar
imagery. By implementing an uncertainty model with finite support along all threecoordinate axes, various uncertainties can be fused into one. Even though pose uncertainty and measurement uncertainty are fundamentally different problems, they
have a similar effect on the mapping process. Namely, they make recovering theposition of objects more difficult, thus unsure. Therefore, the introduced uncertainty model allows to be tuned by the variance of the underlying function, which
results in more or less certainty about the position of the object. Along the elevation dimension, the uncertainty model can be tuned as such that it covers theentire elevation angle of the sonar. Like this, the model is implicitly building theelevation of objects in three dimensions over multiple observations. In comparison to existing techniques, this approach has significant advantages. Probably thebiggest is the ceasing of hard requirements towards the environment or the sensor
pose. The presented method can be used in any environment and regardless of the
motion of the sensor. The resulting sensor model fuses the uncertainties along all
three dimensions in a single formula, where one was derived for the carving andnon-carving case each.
The model validation proved that the derived sensor models work as intended on
66

67

8.2. Future Work

point responses, before the results on the folding chair image showed the advantageous of the non-carving model. Due to the function principle of the sonar, multiple
returns along one beam can occur, which will get overridden by the furthest responsein the carving model. This resulted in high certainty about free space, but did notprove valid for navigation tasks. The non-carving model, however, looked very
promising and the results were as expected. With high certainty about occupancy,
the non-carving model seemed to be a lot better suited for occupancy mapping in
navigation tasks. However, it came with the inverse problem of the carving model,
as no handle to reduce occupancy probability was available.
Following the non-carving modelling approach, several supporting techniques wereintroduced for the model to perform better. The free space compensation provided
a handle to remove artefacts created from noise and to introduce more certainty
about free space. The clamping solved the problem of high values in log-odds space,which are expensive to handle. The implementation of probability scaling createda handle to limit the impact a single measurement can have on the map. This way,
the amount of counter-evidence needed to switch conclusion is implicitly modelled.
The serialization approach introduced reduced the amount of function evaluationsin three dimensions by over 94% and the occupancy threshold allowed to easily
derive a binary map from the calculation probability values. Finally, the concept oftrilinear interpolation was introduced to transfer the probability values from spherical coordinates to the cartesian grid of the map.
Voxblox as a mapping and path planning library offered many valuable tools forfurther testing of the derived sensor model. To integrate the proposed sensor model
into Voxblox, an additional occupancy module was created that takes sonar imagery
as an input, performs the image preprocessing, calculates the probability tensors
and updates the occupancy map. Making use of the integration of Voxblox in ROS,
the modules could be set up such that they work either with true sonar imagery
or with imagery created from a sonar simulator. Rudimentary testing proved theintegration of the occupancy modules valid and further testing can be performed.
In summary, the goals defined in 1.4 have been fulfilled. A novel sensor model was
introduced that overcomes known limitations. The implementation of the derived
sensor model in an occupancy mapping framework was successful and the integration into Voxblox allows now for further testing and research on the behaviour ofthe model.

8.2

Future Work

8.2.1

Optimization ProcessThis thesis ends where the practical testing and optimization process begins. Using the implementation in Voxblox, the sensor model has to be proven valid forsequences of images. Enabling the visualisation tools of Voxblox to display occupancy maps will be a key aspects for understanding the model’s behaviour overmultiple images. Tuning of the model parameters, particularly the model variances,
the scaling factor and the occupancy threshold, will have to be done empirically according to the performance of the model with various input sequences. It is very
likely that, depending on the sonar type and environment, different parameters
might perform better. A trade-off decision would be unavoidable in this case.
Probably the most work lies in the optimization of the computational cost. The
ultimate goal is to build a framework that runs onboard an underwater robotic platform in real time. This requires as well the definition of the sampling rate needed
for proper map building. The final step of testing is then the appliance of a pathfinding algorithm on the occupancy map created from the model. In simulated andChapter 8. Conclusion

68

real cases, this will show how well the model actually performs and how useful thederived maps for navigation tasks are.

8.2.2

Future Research

The proposed approach for the volumetric mapping from sonar imagery has many
advantageous, which have been discussed in previous sections. However, there areas well some serious limitations that require further research. Particularly in thetheoretical derivations, various assumptions have been taken, which might or might
not proof valid for practical usage. In the following, some of the areas of further
research will be presented.
The mathematical derivation of the sensor model was based on a Markov assumption, which states that all previous states are summarized in the current state.
Therefore, the prediction of the next state would only depend on the current state.
This does not need to be true, as there might be factors to the mapping processthat do not fulfil the Markov assumptions. An example for that could be water
current created by tidal motions. The AUV, and thus the sonar sensor, can experience it in ocean environments or brackish waters. Further research could explore
the consequences and positive effects on the sensor model if the Markov assumption
is weakened or rejected.
Another significant limitation is the assumption of independent occupancy voxels.
By ignoring the information that the world is continuous and that certain properties, such as occupancy, usually occur in larger areas, the model loses capabilities indeduction. For example, it is very unlikely that a portion of the world represented
by a voxel is free, if all surrounding voxels are occupied. As a consequence, further
research could be performed on the possibilities to extend the current model to use
this information and make therefore reasonable deductions about the worlds true
state. It has to be mentioned though, that removing the independence assumption
increases the models complexity significantly and the effects on the computational
cost would have to be examined closely.
The approach for the elevation information recovery presented in this work allows
to perform the reconstruction without hard requirements. Compared to existing
techniques, this enables the implementation in various environments and use-cases.
However, the reconstruction of elevation information through uncertainty has its
own set of drawbacks. Most dominantly, the highest occupancy probability is assigned to the points at the centre of the elevation cone, indicated by zero elevation.
As a result, areas of the world that have been rarely seen or not seen at various
elevation levels have imprecise reconstruction and possibly artefacts. Testing andfurther research is required to understand the impact of that limitation on the map
quality and to come up with adequate solutions. Possible approaches could involvethe usage of a different type of uncertainty model with flatter curve or the implementation of multi-hypothesis reasoning for multiple viewpoints.
Finally, the image preprocessing performed in this work focused on the imagery received from Tethys Robotics [3] and is therefore not representative. It is reasonable
to assume that the presented noise countermeasures and downsampling techniques
have their limitations for other environments. Additionally, the analysis on a single
image does not take temporal effects into account, which might create a significant impact. Further research should focus on imagery from various sources andenvironments to get a more complete understanding on how noise can be reduced
effectively. By analysing subsequent images from the same stream, temporal effects
can be extracted that might help to further improve the image preprocessing andthe modelling.

69

8.2. Future Work

Bibliography[1] “Microsoft kinect,” https://developer.microsoft.com/de-de/windows/kinect/,
accessed 11.03.2022.
[2] “Verified market research, market potential underwater robotics,”
https://www.verifiedmarketresearch.com/product/underwater-roboticsmarket/, accessed 11.03.2022.
[3] “Tethys robotics,” https://tethys-robotics.ch/, accessed, 11.03.2022.
[4] H. Cho, B. Kim, and S.-C. Yu, “Auv-based underwater 3-d point cloud generation using acoustic lens-based multibeam sonar,” IEEE Journal of Oceanic
Engineering, vol. 43, no. 4, pp. 856–872, 2018.
[5] M. F. Fallon, J. Folkesson, H. McClelland, and J. J. Leonard, “Relocating
underwater features autonomously using sonar-based slam,” IEEE Journal of
Oceanic Engineering, vol. 38, no. 3, pp. 500–513, 2013.
[6] H. Oleynikova, Z. Taylor, M. Fehr, R. Siegwart, and J. Nieto, “Voxblox: Incremental 3d euclidean signed distance fields for on-board mav planning,” in2017 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), 2017, pp. 1366–1373.
[7] 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, no. 3, pp. 189–206, feb 2013.
[8] M. Nießner, M. Zollhöfer, S. Izadi, and M. Stamminger, “Real-time 3d reconstruction at scale using voxel hashing,” ACM Transactions on Graphics (ToG),
vol. 32, no. 6, pp. 1–11, 2013.
[9] C. Loop, Q. Cai, S. Orts-Escolano, and P. A. Chou, “A closed-form bayesianfusion equation using occupancy probabilities,” in 2016 Fourth International
Conference on 3D Vision (3DV), 2016, pp. 380–388.
[10] P. V. Teixeira, M. Kaess, F. S. Hover, and J. J. Leonard, “Underwater inspection using sonar-based volumetric submaps,” in 2016 IEEE/RSJ International
Conference on Intelligent Robots and Systems (IROS), 2016, pp. 4288–4295.
[11] B. Curless and M. Levoy, “A volumetric method for building complex
models from range images,” in Proceedings of the 23rd Annual Conference on
Computer Graphics and Interactive Techniques, ser. SIGGRAPH ’96. New
York, NY, USA: Association for Computing Machinery, 1996, p. 303–312.
[12] R. A. Newcombe, S. Izadi, O. Hilliges, D. Molyneaux, D. Kim, A. J. Davison,
P. Kohi, J. Shotton, S. Hodges, and A. Fitzgibbon, “Kinectfusion: Real-timedense surface mapping and tracking,” in 2011 10th IEEE International Symposium on Mixed and Augmented Reality, 2011, pp. 127–136.
70

71

Bibliography[13] T. Guerneve and Y. Petillot, “Underwater 3d reconstruction using blueview
imaging sonar,” in OCEANS 2015 - Genova, 2015, pp. 1–7.
[14] R. DeBortoli, A. Nicolai, F. Li, and G. A. Hollinger, “Real-time underwater
3d reconstruction using global context and active labeling,” in 2018 IEEE
International Conference on Robotics and Automation (ICRA), 2018, pp. 6204–
6211.
[15] R. DeBortoli, F. Li, and G. A. Hollinger, “Elevatenet: A convolutional neuralnetwork for estimating the missing dimension in 2d underwater sonar images,”in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), 2019, pp. 8040–8047.
[16] M. D. Aykin and S. Negahdaripour, “On feature matching and image registration for two-dimensional forward-scan sonar imaging,” Journal of Field
Robotics, vol. 30, no. 4, pp. 602–623, may 2013.
[17] S. Negahdaripour, “On 3-d motion estimation from feature tracks in 2-d fs
sonar video,” IEEE Transactions on Robotics, vol. 29, no. 4, pp. 1016–1030,
2013.
[18] S. Negaharipour, “On 3-d scene interpretation from f-s sonar imagery,” in 2012
Oceans, 2012, pp. 1–9.
[19] “Anymal from anybotics,” https://www.anybotics.com/, accessed 22.03.2022.
[20] D. Ribas, P. Ridao, J. Neira, and J. D. Tardos, “Slam using an imaging sonarfor partially structured underwater environments,” in 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2006, pp. 5040–5045.
[21] J. Li, M. Kaess, R. M. Eustice, and M. Johnson-Roberson, “Pose-graph slamusing forward-looking sonar,” IEEE Robotics and Automation Letters, vol. 3,
no. 3, pp. 2330–2337, 2018.
[22] “Blueprint subsea m1200d,” https://www.blueprintsubsea.com/oculus/oculusm-series, accessed 11.03.2022.
[23] “Jw fishers side scan sonar,” http://www.jwfishers.com/products/sss.html, accessed 11.03.2022.
[24] M. Sung, J. Kim, H. Cho, M. Lee, and S.-C. Yu, “Underwater-sonar-imagebased 3d point cloud reconstruction for high data utilization and object
classification using a neural network,” Electronics, vol. 9, no. 11, 2020.
[25] G. Taraldsen, T. A. Reinen, and T. Berg, “The underwater gps problem,” in
OCEANS 2011 IEEE - Spain, 2011, pp. 1–8.
[26] “Minecraft
landscape,”
https://www.cubecraft.net/threads/minecraftlandscape-pics.16287/, accessed 11.03.2022.
[27] T. Nam, J. Shim, and Y. Cho, “A 2.5d map-based mobile robot localization via
cooperation of aerial and ground robots,” Sensors, vol. 17, p. 2730, 11 2017.
[28] S. Thrun, Probabilistic Robotics.

The MIT Press, 2006.

[29] J. McConnell, J. D. Martin, and B. Englot, “Fusing concurrent orthogonal wide-aperture sonar images for dense underwater 3d reconstruction,” in2020 IEEE/RSJ International Conference on Intelligent Robots and Systems(IROS), 2020, pp. 1653–1660.
[30] H. R. Kang, Computational Color Technology.

SPIE, may 2006.

Bibliography

72

Appendix A

Datasheets73

Appendix A. Datasheets

74

Oculus M-Series
Single & Dual Frequency
SonarsThe Oculus M series multibeam sonars are a newgeneration of compact imaging sonar,
designed for use across a wide variety ofunderwater applications.
Their small form factor makes them
ideally suited for deployment onto
micro sized platforms, while
their rugged construction also
makes them an excellent choice
for larger work-class vehicles
and subsea infrastructure.

M370s
With a 200m range capability, this sonar is a costeffective alternative to traditional mechanicallyscanned sonars, typically being used for long range
navigation and situational awareness.

Oculus M1200d
125mm x 122mm x 62mm
360g in water, 980g in air

M750d
A general purpose dual-frequency 750kHz/1.2MHz sonaroffering up to 120 metre range capability for navigation andhigh resolution imagery for near field target identification.

M1200dOffering the highest resolution in the M series range, thedual frequency 1.2MHz/2.1MHz M1200d may be used forspecialised inspection tasks where image quality is criticalwith ranges up to 40m.

M750d 750kHz
15m range

M750d 1.2MHz
Skip/Dumpster
M750d 1.2MHz
Car Tyre & IBC

www.blueprintsubsea.com/oculus62 mm75

M1200d 2.1MHz
5m range
School of Fish

24 mm

2 off M4´0.7
4.5mm deep

70 mm
50 mm
4 off M4´0.7
4.5mm deep

70 mm

121.6 mm

M1200d 2.1MHz
5m range
Scuba Diver

M1200d 1.2MHz
20m range
Heat Exchanger & Pontoon Pilings

Specifications

124.3 mm

Performance

M370s

M750d

BP01041BP01032

BP01042

Operating Frequency

375kHz750kHz / 1.2MHz

1.2MHz / 2.1MHz

Range (max)

200m

120m / 40m

30m / 10m

Range (min)

0.2m

0.1m

0.1m

Range Resolution«

8mm

4mm / 2.5mm

2.5mm / 2.5mm

Update Rate (max)«

40Hz

40Hz

40Hz

Horizontal Aperture

130°

130° / 70°

130° / 60°

Vertical Aperture20°

20° / 12°20° / 12°

18V to 32V isolated DC (12V non-isolated option t)

Number of Beams
(max)

256

512

512

Power Consumption

10W to 35W (model and range dependent «)Angular Resolution

2°

1° / 0.6°

0.6° / 0.4°

Integrated Sensors

Water pressure and temperature(for Velocity-of-Sound calculation)

Beam Separation

0.5°

0.25° / 0.16°

0.25° / 0.16°

Mechanical
Dimensions

125mm Long ´ 122mm Wide ´ 62mm High

Construction

Anodised Aluminium

Weight980g (air), 360g (water)

Depth Rating
Temperature Range

M1200d300m-5°C to +35°C (operating)-20°C to +50°C (storage)

Electrical
Connector
Communications
Supply Voltage

DA-148-P01443-04

Impulse IE55 Series, 6-way
4-wire 100-BaseT Ethernet, 2-wire DSL extender module(Trigger in/out and RS232 aux options t)« indicates parameter is dependent on range
t consult factory for availability and further details

Please note that all functions & specifications may be subject to change in line with our policy of continual product development.

Blueprint Subsea
The Clock Tower Business Centre,
Low Wood, Ulverston, Cumbria,
LA12 8LY, UK+44 (0)1539 531536
enquiries@blueprintsubsea.com