On 2D SLAM for Large Indoor Spaces: A Polygon-Based Solution

On 2D SLAM for Large Indoor Spaces
A Polygon-based Solution
Johann Dichtl
IMT Lille Douai, Univ. Lille
http://car.imt-lille-douai.fr
July 2, 2019
Johann Dichtl PolySLAM July 2, 2019 1 / 70
On 2D SLAM for Large Indoor Spaces
A Polygon-based Solution
Johann Dichtl
IMT Lille Douai, Univ. Lille
http://car.imt-lille-douai.fr
July 2, 2019
2019-07-02
PolySLAM

Introduction
Context & Requirements
Context & Requirements
2019-07-02
PolySLAM
Introduction
• lets start with the context of our work and the requirements that
derive from that

Introduction
Context
Our context: Multi-Robot Indoor Exploration
Requirements towards the map format:
lightweight map format for easy sharing between robots/agents
support for navigation
support for frontier-based exploration
Context
Our context: Multi-Robot Indoor Exploration
Requirements towards the map format:
lightweight map format for easy sharing between robots/agents
support for navigation
support for frontier-based exploration
2019-07-02
PolySLAM
Introduction
Context
• autonomous exploration with robots
• what are todays key issues with this task?
• three key points: lightweight, navigation support, and frontiers
• let’s look at the ﬁrst point: lightweight map format

Introduction
Requirements
Why do we need lightweight map formats?
Requirements
Why do we need lightweight map formats?
2019-07-02
PolySLAM
Introduction
Requirements
• why are lightweight maps important?
• memory bandwidth limits the size of the environment / number of
robots
• as a result, we need to carefully balance between size of the
environment, number of robots in the ﬂeet, and map update
frequency

Introduction
Requirements
Navigation support
Requirements
Navigation support
2019-07-02
PolySLAM
Introduction
Requirements
• navigation relies on path planning
• if we want to avoid navigating through unexplored space, we need a
way to detect when we leave explored space
• if there is no way to tell when we are moving into unexplored space,
we call this map (format) “open”, otherwise “closed”
• the ﬁgure illustrates how this inﬂuences a path planning algorithm
• in a closed map (left), the path only covers explored space
• while on the open map (right), the algorithm creates a different route,
potentially stright through yet undiscovered obstacles

Introduction
Requirements
Why frontiers?
Image source: Marjovi, Ali & Marques, Lino. (2013). Multi-Robot Topological Exploration
Using Olfactory Cues. 10.1007/978-3-642-32723-0_4
Requirements
Why frontiers?
Image source: Marjovi, Ali & Marques, Lino. (2013). Multi-Robot Topological Exploration
Using Olfactory Cues. 10.1007/978-3-642-32723-0_4
2019-07-02
PolySLAM
Introduction
Requirements
• frontiers model the transition from explored into unexplored space in
the absence of obstacles
• in occupancy grids, the frontiers are only implicit – whenever a free
cell borders an unexplored cell, we have a frontier
• frontier-based exploration is not well supported -> more details about
frontiers later

Introduction
State-of-the-Art
State-of-the-Art
2019-07-02
PolySLAM
Introduction
• now that we have seen what our needs are, let’s take a look at the
state-of-the-art map formats that are currently in use

Introduction
State-of-the-Art 2D Map Formats
map
formats
topological
maps
occupancy grids
feature-based
hybridparametric/
geometric
metric
maps
map
formats
topological
maps
occupancy grids
feature-based
hybridparametric/
geometric
metric
maps
2019-07-02
PolySLAM
Introduction
• looking at the state-of-the-art map formats, we ﬁnd the following type
of maps
• the most common 2d map format is the occupancy grid
• feature-based and parametric map formats are less commonly used,
but have a smaller memory footprint in general
• however all of these map formats have one or another undesired
property

Introduction
Occupancy Grids
Occupancy Grids
2019-07-02
PolySLAM
Introduction
Occupancy Grids
• let’s look at the ﬁrst one of this list: the occupancy grid, also known
as grid map
• 2D matrix of states (free/occupied/unknown), can be easily visualized
as 2D image
• as mentioned, this map format is the most common 2D map format in
use today
• unfortunately, it comes with a large memory footprint, making it a less
than ideal choice for multi-robot exploration (map exchange)
• if we look at an open vector map for example -> next slide

Introduction
Vector Map
-1000
-500
0
500
1000
-400 -200 200 400 600 800 1000 1200
t
Image source: Lakaemper et al.: Incremental multi-robot mapping; IROS 2005
Vector Map
-1000
-500
0
500
1000
-400 -200 200 400 600 800 1000 1200
t
Image source: Lakaemper et al.: Incremental multi-robot mapping; IROS 2005
2019-07-02
PolySLAM
Introduction
Vector Map
• we cannot tell exactly at what point we reach unexplored space
• is this the end of the obstacles, or does it continue past this point
• with only the map, we are not able to perform any useful naviagtion
or path planning

Introduction
Feature-Based Map Format
Trajectory
Landmarks
Image source: Kümmerle et al.: g2o: A general framework for graph optimization; ICRA 2011
Abstract— Many popular problems in robotics and computer
vision including various types of simultaneous localization and
mapping (SLAM) or bundle adjustment (BA) can be phrased
as least squares optimization of an error function that can
be represented by a graph. This paper describes the general
structure of such problems and presents g2
o, an open-source
C++ framework for optimizing graph-based nonlinear error
functions. Our system has been designed to be easily extensible
to a wide range of problems and a new problem typically can
be specified in a few lines of code. The current implementation
provides solutions to several variants of SLAM and BA. We
provide evaluations on a wide range of real-world and simulated
datasets. The results demonstrate that while being general g2
o
offers a performance comparable to implementations of state-
of-the-art approaches for the specific problems.
I. INTRODUCTION
A wide range of problems in robotics as well as
in computer-vision involve the minimization of a non-
linear error function that can be represented as a graph.
Typical instances are simultaneous localization and map-
ping (SLAM) [19], [5], [22], [10], [16], [26] or bundle
adjustment (BA) [27], [15], [18]. The overall goal in these
problems is to find the configuration of parameters or state
variables that maximally explain a set of measurements
affected by Gaussian noise. For instance, in graph-based
SLAM the state variables can be either the positions of the
robot in the environment or the location of the landmarks
in the map that can be observed with the robot’s sensors.
Thereby, a measurement depends only on the relative loca-
tion of two state variables, e.g., an odometry measurement
between two consecutive poses depends only on the con-
nected poses. Similarly, in BA or landmark-based SLAM a
measurement of a 3D point or landmark depends only on the
location of the observed point in the world and the position
Trajectory
Landmarks
Trajectory
Landmarks
(a)
(b)
Fig. 1. Real-world datasets processed with our system: The first row of
(a) shows the Victoria Park dataset which consists of 2D odometry and 2D
landmark measurements. The second row of (a) depicts a 3D pose graph of
a multi-level parking garage. While the left images shows the initial states,
the right column depicts the respective result of the optimization process.
Full and zoomed view of the Venice bundle adjustment dataset after being
optimized by our system (b). The dataset consists of 871 camera poses and
2,838,740 projections.
provides acceptable results for most applications. However,
to achieve the maximum performance substantial efforts and
domain knowledge are required.
In this paper, we describe a general framework for per-
forming the optimization of nonlinear least squares problems
Image source: Kümmerle et al.: g2o: A general framework for graph optimization; ICRA 2011
2019-07-02
PolySLAM
Introduction
• feature-based maps have the same problem
• the classic feature-based map format used point-like features, where
each feature is treated as a point, instead of a higher geometric
shape
• same feature-based maps use vectors instead. technically, this is a
hybrid map format
• these maps still only create open maps, and therefore share the
same problem in regard of navigation

Introduction
State-of-the-Art Map Formats
Evaluation
Map format Lightweight Navigation support Frontiers
Grid maps no yes only implicit
Feature-based yes no no
Vector-based yes no no
Evaluation
2019-07-02
PolySLAM
Introduction
• to summarice, none of the common map formats fullﬁls our
requirements
• grid maps are to large, and frontiers are only implicit embedded in
the map format
• feature-based maps are lightweight, but are not suitable for
navigation or exploration
• vector based maps are the same as feature-based: lightweight, but
open

PolyMap
The PolyMap Format
The PolyMap Format
2019-07-02
PolySLAM
PolyMap
• on the previous slide we saw that there is no 2D map format that
satisﬁes our needs

PolyMap
Maps with Vectors
Introducing a map format that is
lightweight
supports navigation (closed)
models frontiers
→ Polygon-Based Map Format: PolyMap
Maps with Vectors
Introducing a map format that is
lightweight
supports navigation (closed)
models frontiers
→ Polygon-Based Map Format: PolyMap
2019-07-02
PolySLAM
PolyMap
Maps with Vectors
• seeing that no map format satisﬁes our needs, we need to develop a
map format
• our approach to this is to take a vector map format and modify it so
that it supports frontiers and ensures that the map is closed
• we add frontiers, by attaching a type to every vector, which
determines wether the vector is an obstacle or a frontier
• and then we build the whole map from polygons, to ensure that the
whole map is closed all the time

PolyMap
Maps with Vectors
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
Maps with Vectors
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
2019-07-02
PolySLAM
PolyMap
Maps with Vectors
• in our case: vector = directed line segment + type
• directed line segments will be important later
• types help to create closed polygons of the environment
• vectors are a sparce model of the environment
• almost all state-of-the-art vector SLAM algorithms create open maps
• “open” means, that there is no clear transition from explored to
unexplored space (save for obstacles)
• closed is important for navigation (path ﬁnding & reachablility)
• closed almost automatically implies frontiers (how to close
otherwise?)
• if we look at an open map for example -> next slide

PolyMap
Vectors with Types
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
Frontiers
Sector
Borders
Obastacles
Vectors with Types
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
Frontiers
Sector
Borders
Obastacles
2019-07-02
PolySLAM
PolyMap
Vectors with Types
• black vectors are obstacles, green vectors are frontiers, and yellow
vectors are sector borders
• sector borders are used to model parts of a polygon that is connect
with another polygin without any obstacle in the way
• in other words, sector borders are neither obstacles, nor do they
mark a transition into unexplored space
• we will see mote on that topic later
• we usually don’t draw arrow heads on sector borders, since often this
is more confusing then helping, given that sector borders are be
construction always overlapping with other sector borders

PolySLAM Overview
The PolySLAM Algorithm
The PolySLAM Algorithm
2019-07-02
PolySLAM
PolySLAM Overview
• now that we have speciﬁed a map format that fullﬁlls our wish list, we
need a SLAM algorithm that provides us with PolyMaps as output

PolySLAM Overview
PolySLAM
What is Simultaneous Localization and Mapping (SLAM)?
sensor data
SLAM
pose estimate
INPUT
PROCESSING
OUTPUT global map
PolySLAM
What is Simultaneous Localization and Mapping (SLAM)?
sensor data
SLAM
pose estimate
INPUT
PROCESSING
OUTPUT global map
2019-07-02
PolySLAM
PolySLAM Overview
PolySLAM
• SLAM is the task of creating a map and determining the current
location of the robot within the map live.
• in robotics, instead of live, we usually say online, as opposed to
ofﬂine (which would mean to do so after all the data has already
been collected)
• the very basic concept of SLAM looks like this
• basic input/output concept (blackbox view)
• chicken-and-egg problem with localization and mapping

PolySLAM Overview
PolySLAM Overview
Video: PolySLAM in action
PolySLAM Overview
Video: PolySLAM in action
2019-07-02
PolySLAM
PolySLAM Overview
PolySLAM Overview
• show the video, explaining the various stages

PolySLAM Overview
PolySLAM Overview
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
pointcloud pose estimate
pointcloud
polygon
polygon/PolyMap
PolyMappose estimate
laser scan odometry
map
PolySLAM Overview
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
pointcloud pose estimate
pointcloud
polygon
polygon/PolyMap
PolyMappose estimate
laser scan odometry
map
2019-07-02
PolySLAM
PolySLAM Overview
PolySLAM Overview
• this is what the inside of the “blackbox” looks like with PolySLAM
• still the same input/output
• sensor data is split into odometry & laser
• some data is used recurently (pose estimate & map estimate)

PolySLAM Overview
PolySLAM Step 1: Data Acquisition
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
2019-07-02
PolySLAM
PolySLAM Overview
• this shows the robot in the environment (not the map)
• we see an idealized world with no meassurement errors
• this is a toy example designed for good visualization
• real lasers have much more beams / higher resolution

PolySLAM Overview
PolySLAM Step 2: Data Alignment
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: setup
robot
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: setup
robot
2019-07-02
PolySLAM
PolySLAM Overview
• zoomed in for better visualization
• odometry delta (since last keyframe) is used to pre-align the point
cloud
• if this is the ﬁrst keyframe, we can chose the start position (usualy
0@0)

PolySLAM Overview
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: laser scan data
robot
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: laser scan data
robot
2019-07-02
PolySLAM
PolySLAM Overview
• if we don’t have a map yet, we skip this step entirely
• if we do have a map, we align the point cloud with the map
• this is done with ICP, a least-squares optimization method
• the ﬁgure now shows the map, not the environment (hence arrows on
the black lines)

PolySLAM Overview
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: aligned point cloud
robot
PolyMap Merging
Polygon Refinement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: aligned point cloud
robot
2019-07-02
PolySLAM
PolySLAM Overview
• after this optimization step, the point cloud is aligned
• we are using a modified version of the ICP algorithm, and talk about
details later
• often, there is only a partial overlap between map and pointcloud,
since we are exploring new parts of the environment
• now the alignment step is finished
• the gray marker shows the pose estimate before the alignment
• the new alignement (blue) is the first output of our SLAM algorithm:
the pose estimate

PolySLAM Overview
Iterative Closest Point (ICP)
Our variant:
2D; least squares; equal weighted points,
threshold-based outlier rejection
point-to-vector corresponding point pairing
backface culling
PolyMap Merging
Polygon Refinement
Keyframe Creation
Data Alignment
Data Acquisition
Iterative Closest Point (ICP)
Our variant:
2D; least squares; equal weighted points,
threshold-based outlier rejection
point-to-vector corresponding point pairing
backface culling
2019-07-02
PolySLAM
PolySLAM Overview
• widely used, many variants exist
• ICP uses corresponding pairs of points to align source data with
target data
• the classic ICP algorithm takes two point clouds (source and target)
• first every point from source is paired with the closest point in target
• then outliers are rejected, and we determine the transformation that
minimizes the square distance error over all pairs
• repeated multiple times until satisfied
• we are using 2D least square equal weighted points (all inliers are
treated equal)
• inlier-outliers are determined by a simple threshold (tunable
parameter)
• backface culling: more details in a few slides

PolySLAM Overview
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: match pairs
robot
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: match pairs
robot
2019-07-02
PolySLAM
PolySLAM Overview
• instead of having a target point cloud, we have a vector collection
(the map)
• consequently, we cannot pair points like in the standard algorithm
• instead we use the projection point (or end point, if the projection
point lies outside of the vector)
• the rest works like normal ICP, except for backface culling -> next
slide

PolySLAM Overview
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
ICP example - laser scan
robot
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
ICP example - laser scan
robot
2019-07-02
PolySLAM
PolySLAM Overview
• if we are unlucky, our pre-aligned point cloud might be closer to the
back of the wall
• here, our ICP algorithm could end up in a local minimum with the
point cloud snapped to the wrong side of the wall

PolySLAM Overview
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
ICP example - without backface culling
robot
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
ICP example - without backface culling
robot
2019-07-02
PolySLAM
PolySLAM Overview
• like this
• obvously, this in undesireable
• our solution to this problem is to disregard any vectors of our map
that are facing away from the robot
• by this, i mean that the vectors normal is pointing away from the
sensor center
• remember, our vectors are directed line segemnts, otherwise we
would not be able to use this technique
• we call this technique “backface culling”, based on a technique in 3D
graphics, were polygons in a similar fashion ignored if their normal is
pointing away from the camera
• to the best of our knowledge, there are no other 2D SLAM algorithms
that make use of this technique

PolySLAM Overview
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
ICP example - with backface culling
robot
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
robot
2019-07-02
PolySLAM
PolySLAM Overview
• with backface culling active, the gray vectors are now ignored when
the algorithm is computing the corresponding points
• and the aligned point cloud looks like this -> next slide

PolySLAM Overview
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
robot
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
robot
2019-07-02
PolySLAM
PolySLAM Overview
• this technique does not garanty that we ﬁnd the global minimum
• however we have shown in experiments that it improves the overall
results (depending on environment and robot hardware, obviously)

PolySLAM Overview
PolySLAM Step 3: Keyframe Creation
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
2019-07-02
PolySLAM
PolySLAM Overview
• with the pointcloud aligned we now have to create a keyframe
• in our case a keyframe is a polygon / PolyMap
• to create a polygon we connect the point cloud “in order”, i.e. in the
same order that we received the points

PolySLAM Overview
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
19 vectors
PolyMap Merging
Polygon Refinement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
19 vectors
2019-07-02
PolySLAM
PolySLAM Overview
• the last point of the point cloud is connect to the sensor center and
the sensor center is connected to the first point from the point cloud
• by doing this, we get a closed polygon
• vector types is choosen based on vector length
• vectors connected with the sensor center are always frontiers
• if we would have a laser with a 360◦
FOV, we could also just
connected the last and first point of the point cloud, skipping the
sensor center (no problems down the line in our SLAM algorithm)

PolySLAM Overview
PolySLAM Step 4: Polygon Refinement
the basic algorithm:
1 collect vectors as vector chains
2 perform line fitting & scoring
3 select highest scoring line fitting, and remove inliers from
vector chain
4 repeat until all vectors are covered
5 create refined vectors from fitting results
6 build new polygon from refined vectors
PolyMap Merging
Polygon Refinement
Keyframe Creation
Data Alignment
Data Acquisition
the basic algorithm:
1 collect vectors as vector chains
2 perform line fitting & scoring
3 select highest scoring line fitting, and remove inliers from
vector chain
4 repeat until all vectors are covered
5 create refined vectors from fitting results
6 build new polygon from refined vectors
2019-07-02
PolySLAM
PolySLAM Overview
• explain the algorithm: lienear regression over vector chains + scoring
function
• ignoring the algorithm, what would we like to see as a result. how
does the algorithm get us there

PolySLAM Overview
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
1
2
3
4
5
6
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
1
2
3
4
5
6
2019-07-02
PolySLAM
PolySLAM Overview
• the newly created polygon has a high vector count by construction
• we want to reduce the vector count without reducing the quality
• in fact, we would like to improve the quality
• basic idea: ﬁt lines into the vectors/point cloud to reduce the noice
• we use linear regression, more details later

PolySLAM Overview
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
2019-07-02
PolySLAM
PolySLAM Overview

PolySLAM Overview
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
2019-07-02
PolySLAM
PolySLAM Overview

PolySLAM Overview
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
8 vectors
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
8 vectors
2019-07-02
PolySLAM
PolySLAM Overview

PolySLAM Overview
PolySLAM Step 5: PolyMap Merging
0
1
2
3
4
5
0 1 2 3 4 5 6
PolyMap Merging
Polygon Refinement
Keyframe Creation
Data Alignment
Data Acquisition
0
1
2
3
4
5
0 1 2 3 4 5 6
2019-07-02
PolySLAM
PolySLAM Overview
• we need a lot od nearest neighbor search, hence we need a data
structure that supports this
• BSP trees fit our needs
• by using BSP trees, we will eventually have to split up our polygons
into smaller polygons
• in this figure we can see a map that already is split up into smaller
parts
• if we ant to insert the next keyframe -> next slide

PolySLAM Overview
0
1
2
3
4
5
0 1 2 3 4 5 6
Keyframe #6 on Map
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
1
2
3
4
5
0 1 2 3 4 5 6
Keyframe #6 on Map
2019-07-02
PolySLAM
PolySLAM Overview
• like this one, we end up with a new map like this -> next slide

PolySLAM Overview
0
1
2
3
4
5
0 1 2 3 4 5 6
PolyMap Merging
Polygon Reﬁnement
Keyframe Creation
Data Alignment
Data Acquisition
0
1
2
3
4
5
0 1 2 3 4 5 6
2019-07-02
PolySLAM
PolySLAM Overview
• parts that are already in the map are removed from the keyframe and
only what is new is added to the map
• in our case, the top of the keyframe is cut off, and the lower part is
inserted into the BSP tree
• we can also see that some of the BSP tree’s hyperplanes are
splitting up the new part into smaller polygons
• in general that is no problem, but we still use a grid structure to keep
this effect local. more aobut this later

Topological Maps
Topological Maps
Topological Maps
2019-07-02
PolySLAM
Topological Maps
• now that we have a working SLAM algorithm, we need a topological
map to enable navigation on PolyMaps
• when we talk about topological maps, we usually refer to topological
graphs, since that is a suitable data structure for the task
• a topological graph is a graph, where the nodes are linked with areas
of our (geometric) map
• and if there is a direct connection between two nodes (without
crossing any third node/area), then we connect the two nodes with
an edge
• with this, it is easy to compute if an area is reachable from the robot’s
current position, and which path is the shortest
• however “shortest” is not always accurate – that depends a lot on
how the map is split up into areas for the topological map, and what
assumptions we make for the robot

Topological Maps
Topological Maps
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
-0.5 0 0.5 1 1.5 2 2.5 3 3.5
Robot
Topological Maps
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
-0.5 0 0.5 1 1.5 2 2.5 3 3.5
Robot
2019-07-02
PolySLAM
Topological Maps
Topological Maps
• let’s look at this simple example
• we have a robot in the bottom right area
• lets add a target, or goal, and view at this from a “Navigation” point of
view -> next slide

Topological Maps
Topological Maps
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
-0.5 0 0.5 1 1.5 2 2.5 3 3.5
start
goal
Topological Maps
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
-0.5 0 0.5 1 1.5 2 2.5 3 3.5
start
goal
2019-07-02
PolySLAM
Topological Maps
Topological Maps
• we now have a start location and a gial that we want to reach
• but right now, we don’t have a topological map yet. lets add one ->
next slide

Topological Maps
Topological Maps
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
-0.5 0 0.5 1 1.5 2 2.5 3 3.5
Topological Maps
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
-0.5 0 0.5 1 1.5 2 2.5 3 3.5
2019-07-02
PolySLAM
Topological Maps
Topological Maps
• we now have the graph, with its nodes and edges
• but we can also see the sector borders, so that we can see where
each area ends
• in this toy example, we have a low number of polygons, and relatively
large areas as a consequence
• in maps from real data, we can expect a lot more and a lot smaller
polygons
• still, our topological graph is typically much smaller than a graph that
is build from a grid map
• but back to navigation. with the graph now present, we can perform a
simple graph search to ﬁnd a path from start to goal

Topological Maps
Topological Maps
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
-0.5 0 0.5 1 1.5 2 2.5 3 3.5
start
goal
Topological Maps
-0.5
0
0.5
1
1.5
2
2.5
3
3.5
-0.5 0 0.5 1 1.5 2 2.5 3 3.5
start
goal
2019-07-02
PolySLAM
Topological Maps
Topological Maps
• we are using the A∗
algorithm to ﬁnd the shortest path
• with this, we can compute the global path from any point in the map
to any other point in the map

Validation
Validation
Validation
2019-07-02
PolySLAM
Validation
•

Validation
Implementation
Implementation in Pharo (Smalltalk)
Frameworks and middle-ware used:
ROS (Robot Operation System)
PhaROS (Pharo ↔ ROS bridge)
Gnuplot (visualization only)
Implementation
Implementation in Pharo (Smalltalk)
Frameworks and middle-ware used:
ROS (Robot Operation System)
PhaROS (Pharo ↔ ROS bridge)
Gnuplot (visualization only)
2019-07-02
PolySLAM
Validation
Implementation
• ROS is the backbone to control our robots

Validation
Simulation Setup
ROS + Gazebo (simulator)
Differential-Drive Robot model + laser scanner
Simulation Setup
ROS + Gazebo (simulator)
Differential-Drive Robot model + laser scanner
2019-07-02
PolySLAM
Validation
Simulation Setup
•

Validation
Simulator: Willow Garage Environment
0
10
20
30
40
-10 0 10 20 30 40 50
otf=0.5, ost=0.2, mi=40, st=0.0004, od=0.2
distance traveled=950.8, time spent=1h 54m 59.78s
max error: dist=0.177, angle=0.022
0
10
20
30
40
-10 0 10 20 30 40 50
otf=0.5, ost=0.2, mi=40, st=0.0004, od=0.2
distance traveled=950.8, time spent=1h 54m 59.78s
max error: dist=0.177, angle=0.022
2019-07-02
PolySLAM
Validation
• map size: 55x45m
• distance travelled: 951m
• max errors: 0.18m / 0.022◦
• max relative translation error: 0.019%

Validation
map size: 55x45m
distance traveled: 951m
max. errors: 0.18m / 0.022◦
max. translation error, relative to total trajectory: 0.019%
map size: 55x45m
distance traveled: 951m
max. errors: 0.18m / 0.022◦
max. translation error, relative to total trajectory: 0.019%
2019-07-02
PolySLAM
Validation
• distance travelled: 951m
• max errors: 0.18m / 0.022◦
• max relative translation error: 0.019%

Validation
Real Robot & Environment Setup
Robot: Turtlebot 2
Laser Scanner: Hokuyo UTM-30LX (270◦ FOV, 40 FPS, 30m range)
Robot: Turtlebot 2
Laser Scanner: Hokuyo UTM-30LX (270◦ FOV, 40 FPS, 30m range)
2019-07-02
PolySLAM
Validation
•

Validation
Department Informatics & Automation
2019-07-02
PolySLAM
Validation
• trajectory: ca. 70m

Validation
2019-07-02
PolySLAM
Validation
• left: PolySLAM
• right: Karto

Validation
Inria Labratory
-5
0
5
10
15
-5 0 5 10 15 20 25 30 35 40
Inria Labratory
-5
0
5
10
15
-5 0 5 10 15 20 25 30 35 40
2019-07-02
PolySLAM
Validation
Inria Labratory
• trajectory: 222m
• estimated error: < 0.5m

Validation
Experiments
Result:
the maps are consistent
the pose errors are small
the maps are suitable for navigation tasks
Experiments
Result:
the maps are consistent
the pose errors are small
the maps are suitable for navigation tasks
2019-07-02
PolySLAM
Validation
Experiments
•

Conclusion
Conclusion
Conclusion
2019-07-02
PolySLAM
Conclusion
•

Conclusion
Conclusion: Map Formats
The PolyMap Format satisfies all our requirements
PolyMap yes yes yes
The PolyMap Format satisfies all our requirements
PolyMap yes yes yes
2019-07-02
PolySLAM
Conclusion
• to summarice, none of the common map formats fullfils our
requirements
• grid maps are to large, and frontiers are only implicit embedded in
the map format
• feature-based maps are lightweight, but are not suitable for
navigation or exploration
• vector based maps are the same as feature-based: lightweight, but
open

Conclusion
Conclusion PolySLAM
Our PolySLAM implementation
implemented in Pharo in a test-driven approach
creates PolyMaps from standard input (laser & odometry)
is capable of creating consistent maps even without global
optimization
has proven itself both in simulations and with real robots
Conclusion PolySLAM
Our PolySLAM implementation
implemented in Pharo in a test-driven approach
creates PolyMaps from standard input (laser & odometry)
is capable of creating consistent maps even without global
optimization
has proven itself both in simulations and with real robots
2019-07-02
PolySLAM
Conclusion
Conclusion PolySLAM
•

Conclusion
Published Papers:
ICIRA 2018: Covers the PolyMap format and its advantages
over other map formats
ICARSC 2019 Focuses on PolySLAM, and the resulting map
quality
IntelliSys 2019: Presents navigation via topological graphs
build from PolyMaps
Published Papers:
ICIRA 2018: Covers the PolyMap format and its advantages
over other map formats
ICARSC 2019 Focuses on PolySLAM, and the resulting map
quality
IntelliSys 2019: Presents navigation via topological graphs
build from PolyMaps
2019-07-02
PolySLAM
Conclusion
•

Outlook
Outlook
Outlook
2019-07-02
PolySLAM
Outlook
•

Outlook
Outlook
What is still missing, what can be improved?
Global optimization, e.g. via pose graph optimization
Cross-keyframe optimization
Dynamic outlier threshold for the polygon reﬁnement
Speed / Performance
Collaborative multi-robot SLAM
Outlook
What is still missing, what can be improved?
Global optimization, e.g. via pose graph optimization
Cross-keyframe optimization
Dynamic outlier threshold for the polygon reﬁnement
Speed / Performance
Collaborative multi-robot SLAM
2019-07-02
PolySLAM
Outlook
Outlook
•

ﬁn
Thanks
Thanks
2019-07-02
PolySLAM
ﬁn

Appendix
2019-07-02
PolySLAM
Appendix

On 2D SLAM for Large Indoor Spaces: A Polygon-Based Solution

Recomendados

Recomendados

Mais conteúdo relacionado

Semelhante a On 2D SLAM for Large Indoor Spaces: A Polygon-Based Solution

Semelhante a On 2D SLAM for Large Indoor Spaces: A Polygon-Based Solution (20)

Mais de Noury Bouraqadi

Mais de Noury Bouraqadi (14)

Último

Último (20)

On 2D SLAM for Large Indoor Spaces: A Polygon-Based Solution