Successfully reported this slideshow.
We use your LinkedIn profile and activity data to personalize ads and to show you more relevant ads. You can change your ad preferences anytime.

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

123 views

Published on

Slides + comments + appendices of the PhD defense of Johann Dichtl. The validation relied on ROS and Pharo. More info and demo video at: https://noury.tech/projects/polygon-based-slam/

Published in: Technology
  • DOWNLOAD FULL BOOKS, INTO AVAILABLE FORMAT ......................................................................................................................... ......................................................................................................................... 1.DOWNLOAD FULL. PDF EBOOK here { https://tinyurl.com/yxufevpm } ......................................................................................................................... 1.DOWNLOAD FULL. EPUB Ebook here { https://tinyurl.com/yxufevpm } ......................................................................................................................... 1.DOWNLOAD FULL. doc Ebook here { https://tinyurl.com/yxufevpm } ......................................................................................................................... 1.DOWNLOAD FULL. PDF EBOOK here { https://tinyurl.com/yxufevpm } ......................................................................................................................... 1.DOWNLOAD FULL. EPUB Ebook here { https://tinyurl.com/yxufevpm } ......................................................................................................................... 1.DOWNLOAD FULL. doc Ebook here { https://tinyurl.com/yxufevpm } ......................................................................................................................... ......................................................................................................................... ......................................................................................................................... .............. Browse by Genre Available eBooks ......................................................................................................................... Art, Biography, Business, Chick Lit, Children's, Christian, Classics, Comics, Contemporary, Cookbooks, Crime, Ebooks, Fantasy, Fiction, Graphic Novels, Historical Fiction, History, Horror, Humor And Comedy, Manga, Memoir, Music, Mystery, Non Fiction, Paranormal, Philosophy, Poetry, Psychology, Religion, Romance, Science, Science Fiction, Self Help, Suspense, Spirituality, Sports, Thriller, Travel, Young Adult,
       Reply 
    Are you sure you want to  Yes  No
    Your message goes here
  • Be the first to like this

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

  1. 1. 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
  2. 2. Introduction Context & Requirements Johann Dichtl PolySLAM July 2, 2019 2 / 70 Context & Requirements 2019-07-02 PolySLAM Introduction • lets start with the context of our work and the requirements that derive from that
  3. 3. 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 Johann Dichtl PolySLAM July 2, 2019 3 / 70 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 first point: lightweight map format
  4. 4. Introduction Requirements Why do we need lightweight map formats? Johann Dichtl PolySLAM July 2, 2019 4 / 70 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 fleet, and map update frequency
  5. 5. Introduction Requirements Navigation support Johann Dichtl PolySLAM July 2, 2019 5 / 70 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 figure illustrates how this influences 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
  6. 6. 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 Johann Dichtl PolySLAM July 2, 2019 6 / 70 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
  7. 7. Introduction State-of-the-Art Johann Dichtl PolySLAM July 2, 2019 7 / 70 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
  8. 8. Introduction State-of-the-Art 2D Map Formats map formats topological maps occupancy grids feature-based hybridparametric/ geometric metric maps Johann Dichtl PolySLAM July 2, 2019 8 / 70 State-of-the-Art 2D Map Formats map formats topological maps occupancy grids feature-based hybridparametric/ geometric metric maps 2019-07-02 PolySLAM Introduction State-of-the-Art 2D Map Formats • looking at the state-of-the-art map formats, we find 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
  9. 9. Introduction Occupancy Grids Johann Dichtl PolySLAM July 2, 2019 9 / 70 Occupancy Grids 2019-07-02 PolySLAM Introduction Occupancy Grids • let’s look at the first 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
  10. 10. 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 Johann Dichtl PolySLAM July 2, 2019 10 / 70 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
  11. 11. Introduction Feature-Based Map Format Trajectory Landmarks Image source: Kümmerle et al.: g2o: A general framework for graph optimization; ICRA 2011 Johann Dichtl PolySLAM July 2, 2019 11 / 70 Feature-Based Map Format 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 Map Format • 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
  12. 12. 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 Johann Dichtl PolySLAM July 2, 2019 12 / 70 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 2019-07-02 PolySLAM Introduction State-of-the-Art Map Formats • 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
  13. 13. PolyMap The PolyMap Format Johann Dichtl PolySLAM July 2, 2019 13 / 70 The PolyMap Format 2019-07-02 PolySLAM PolyMap • on the previous slide we saw that there is no 2D map format that satisfies our needs
  14. 14. PolyMap Maps with Vectors Introducing a map format that is lightweight supports navigation (closed) models frontiers → Polygon-Based Map Format: PolyMap Johann Dichtl PolySLAM July 2, 2019 14 / 70 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 satisfies 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
  15. 15. PolyMap Maps with Vectors 0 0.5 1 1.5 2 2.5 3 3.5 4 0 1 2 3 4 5 Johann Dichtl PolySLAM July 2, 2019 15 / 70 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 finding & reachablility) • closed almost automatically implies frontiers (how to close otherwise?) • if we look at an open map for example -> next slide
  16. 16. 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 Johann Dichtl PolySLAM July 2, 2019 16 / 70 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
  17. 17. PolySLAM Overview The PolySLAM Algorithm Johann Dichtl PolySLAM July 2, 2019 17 / 70 The PolySLAM Algorithm 2019-07-02 PolySLAM PolySLAM Overview • now that we have specified a map format that fullfills our wish list, we need a SLAM algorithm that provides us with PolyMaps as output
  18. 18. PolySLAM Overview PolySLAM What is Simultaneous Localization and Mapping (SLAM)? sensor data SLAM pose estimate INPUT PROCESSING OUTPUT global map Johann Dichtl PolySLAM July 2, 2019 18 / 70 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 offline (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
  19. 19. PolySLAM Overview PolySLAM Overview Video: PolySLAM in action Johann Dichtl PolySLAM July 2, 2019 19 / 70 PolySLAM Overview Video: PolySLAM in action 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Overview • show the video, explaining the various stages
  20. 20. PolySLAM Overview PolySLAM Overview PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition pointcloud pose estimate pointcloud polygon polygon/PolyMap PolyMappose estimate laser scan odometry map Johann Dichtl PolySLAM July 2, 2019 20 / 70 PolySLAM Overview PolyMap Merging Polygon Refinement 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)
  21. 21. 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 Johann Dichtl PolySLAM July 2, 2019 21 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition 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 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 1: Data Acquisition • 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
  22. 22. 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 Johann Dichtl PolySLAM July 2, 2019 22 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition 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 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 2: Data Alignment • zoomed in for better visualization • odometry delta (since last keyframe) is used to pre-align the point cloud • if this is the first keyframe, we can chose the start position (usualy 0@0)
  23. 23. 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: laser scan data robot Johann Dichtl PolySLAM July 2, 2019 23 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition 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: laser scan data robot 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 2: Data Alignment • 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 figure now shows the map, not the environment (hence arrows on the black lines)
  24. 24. 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: aligned point cloud robot Johann Dichtl PolySLAM July 2, 2019 24 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition 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: aligned point cloud robot 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 2: Data Alignment • 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
  25. 25. PolySLAM Overview PolySLAM Step 2: Data Alignment Iterative Closest Point (ICP) Our variant: 2D; least squares; equal weighted points, threshold-based outlier rejection point-to-vector corresponding point pairing backface culling Johann Dichtl PolySLAM July 2, 2019 25 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 2: Data Alignment 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 PolySLAM Step 2: Data Alignment • 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
  26. 26. 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: match pairs robot Johann Dichtl PolySLAM July 2, 2019 26 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition 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: match pairs robot 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 2: Data Alignment • 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
  27. 27. PolySLAM Overview PolySLAM Step 2: Data Alignment 0 0.5 1 1.5 2 2.5 1 1.5 2 2.5 3 3.5 4 ICP example - laser scan robot Johann Dichtl PolySLAM July 2, 2019 27 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 2: Data Alignment 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 PolySLAM Step 2: Data Alignment • 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
  28. 28. PolySLAM Overview PolySLAM Step 2: Data Alignment 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 Johann Dichtl PolySLAM July 2, 2019 28 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 2: Data Alignment 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 PolySLAM Step 2: Data Alignment • 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
  29. 29. PolySLAM Overview PolySLAM Step 2: Data Alignment 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 Johann Dichtl PolySLAM July 2, 2019 29 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 2: Data Alignment 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 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 2: Data Alignment • 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
  30. 30. PolySLAM Overview PolySLAM Step 2: Data Alignment 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 Johann Dichtl PolySLAM July 2, 2019 30 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 2: Data Alignment 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 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 2: Data Alignment • this technique does not garanty that we find the global minimum • however we have shown in experiments that it improves the overall results (depending on environment and robot hardware, obviously)
  31. 31. 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 Johann Dichtl PolySLAM July 2, 2019 31 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition 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 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 3: Keyframe Creation • 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
  32. 32. 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 19 vectors Johann Dichtl PolySLAM July 2, 2019 32 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition 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 19 vectors 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 3: Keyframe Creation • 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)
  33. 33. 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 Johann Dichtl PolySLAM July 2, 2019 33 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition 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 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 4: Polygon Refinement • 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
  34. 34. PolySLAM Overview PolySLAM Step 4: Polygon Refinement 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 Johann Dichtl PolySLAM July 2, 2019 34 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 4: Polygon Refinement 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 PolySLAM Step 4: Polygon Refinement • 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: fit lines into the vectors/point cloud to reduce the noice • we use linear regression, more details later
  35. 35. PolySLAM Overview PolySLAM Step 4: Polygon Refinement 0 0.5 1 1.5 2 2.5 3 3.5 4 0 1 2 3 4 5 robot Johann Dichtl PolySLAM July 2, 2019 35 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 4: Polygon Refinement 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 Step 4: Polygon Refinement • 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: fit lines into the vectors/point cloud to reduce the noice • we use linear regression, more details later
  36. 36. PolySLAM Overview PolySLAM Step 4: Polygon Refinement 0 0.5 1 1.5 2 2.5 3 3.5 4 0 1 2 3 4 5 Johann Dichtl PolySLAM July 2, 2019 36 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 4: Polygon Refinement 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 Step 4: Polygon Refinement • 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: fit lines into the vectors/point cloud to reduce the noice • we use linear regression, more details later
  37. 37. PolySLAM Overview PolySLAM Step 4: Polygon Refinement 0 0.5 1 1.5 2 2.5 3 3.5 4 0 1 2 3 4 5 robot 8 vectors Johann Dichtl PolySLAM July 2, 2019 37 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 4: Polygon Refinement 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 Step 4: Polygon Refinement • 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: fit lines into the vectors/point cloud to reduce the noice • we use linear regression, more details later
  38. 38. PolySLAM Overview PolySLAM Step 5: PolyMap Merging 0 1 2 3 4 5 0 1 2 3 4 5 6 Johann Dichtl PolySLAM July 2, 2019 38 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 5: PolyMap Merging 0 1 2 3 4 5 0 1 2 3 4 5 6 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 5: PolyMap Merging • 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
  39. 39. PolySLAM Overview PolySLAM Step 5: PolyMap Merging 0 1 2 3 4 5 0 1 2 3 4 5 6 Keyframe #6 on Map Johann Dichtl PolySLAM July 2, 2019 39 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 5: PolyMap Merging 0 1 2 3 4 5 0 1 2 3 4 5 6 Keyframe #6 on Map 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 5: PolyMap Merging • like this one, we end up with a new map like this -> next slide
  40. 40. PolySLAM Overview PolySLAM Step 5: PolyMap Merging 0 1 2 3 4 5 0 1 2 3 4 5 6 Johann Dichtl PolySLAM July 2, 2019 40 / 70 PolyMap Merging Polygon Refinement Keyframe Creation Data Alignment Data Acquisition PolySLAM Step 5: PolyMap Merging 0 1 2 3 4 5 0 1 2 3 4 5 6 2019-07-02 PolySLAM PolySLAM Overview PolySLAM Step 5: PolyMap Merging • 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
  41. 41. Topological Maps Topological Maps Johann Dichtl PolySLAM July 2, 2019 41 / 70 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
  42. 42. 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 Johann Dichtl PolySLAM July 2, 2019 42 / 70 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
  43. 43. 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 Johann Dichtl PolySLAM July 2, 2019 43 / 70 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
  44. 44. 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 Johann Dichtl PolySLAM July 2, 2019 44 / 70 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 find a path from start to goal
  45. 45. 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 Johann Dichtl PolySLAM July 2, 2019 45 / 70 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 find the shortest path • with this, we can compute the global path from any point in the map to any other point in the map
  46. 46. Validation Validation Johann Dichtl PolySLAM July 2, 2019 46 / 70 Validation 2019-07-02 PolySLAM Validation •
  47. 47. Validation Implementation Implementation in Pharo (Smalltalk) Frameworks and middle-ware used: ROS (Robot Operation System) PhaROS (Pharo ↔ ROS bridge) Gnuplot (visualization only) Johann Dichtl PolySLAM July 2, 2019 47 / 70 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
  48. 48. Validation Simulation Setup ROS + Gazebo (simulator) Differential-Drive Robot model + laser scanner Johann Dichtl PolySLAM July 2, 2019 48 / 70 Simulation Setup ROS + Gazebo (simulator) Differential-Drive Robot model + laser scanner 2019-07-02 PolySLAM Validation Simulation Setup •
  49. 49. 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 Johann Dichtl PolySLAM July 2, 2019 49 / 70 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 2019-07-02 PolySLAM Validation Simulator: Willow Garage Environment • map size: 55x45m • distance travelled: 951m • max errors: 0.18m / 0.022◦ • max relative translation error: 0.019%
  50. 50. Validation Simulator: Willow Garage Environment map size: 55x45m distance traveled: 951m max. errors: 0.18m / 0.022◦ max. translation error, relative to total trajectory: 0.019% Johann Dichtl PolySLAM July 2, 2019 50 / 70 Simulator: Willow Garage Environment 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 Simulator: Willow Garage Environment • map size: 55x45m • distance travelled: 951m • max errors: 0.18m / 0.022◦ • max relative translation error: 0.019%
  51. 51. Validation Real Robot & Environment Setup Robot: Turtlebot 2 Laser Scanner: Hokuyo UTM-30LX (270◦ FOV, 40 FPS, 30m range) Johann Dichtl PolySLAM July 2, 2019 51 / 70 Real Robot & Environment Setup Robot: Turtlebot 2 Laser Scanner: Hokuyo UTM-30LX (270◦ FOV, 40 FPS, 30m range) 2019-07-02 PolySLAM Validation Real Robot & Environment Setup •
  52. 52. Validation Department Informatics & Automation Johann Dichtl PolySLAM July 2, 2019 52 / 70 Department Informatics & Automation 2019-07-02 PolySLAM Validation Department Informatics & Automation • trajectory: ca. 70m
  53. 53. Validation Johann Dichtl PolySLAM July 2, 2019 53 / 70 2019-07-02 PolySLAM Validation • left: PolySLAM • right: Karto
  54. 54. Validation Inria Labratory -5 0 5 10 15 -5 0 5 10 15 20 25 30 35 40 Johann Dichtl PolySLAM July 2, 2019 54 / 70 Inria Labratory -5 0 5 10 15 -5 0 5 10 15 20 25 30 35 40 2019-07-02 PolySLAM Validation Inria Labratory • map size: 45x20m • trajectory: 222m • estimated error: < 0.5m
  55. 55. Validation Experiments Result: the maps are consistent the pose errors are small the maps are suitable for navigation tasks Johann Dichtl PolySLAM July 2, 2019 55 / 70 Experiments Result: the maps are consistent the pose errors are small the maps are suitable for navigation tasks 2019-07-02 PolySLAM Validation Experiments •
  56. 56. Conclusion Conclusion Johann Dichtl PolySLAM July 2, 2019 56 / 70 Conclusion 2019-07-02 PolySLAM Conclusion •
  57. 57. Conclusion Conclusion: Map Formats The PolyMap Format satisfies all our requirements Map format Lightweight Navigation support Frontiers Grid maps no yes only implicit Feature-based yes no no Vector-based yes no no PolyMap yes yes yes Johann Dichtl PolySLAM July 2, 2019 57 / 70 Conclusion: Map Formats The PolyMap Format satisfies all our requirements Map format Lightweight Navigation support Frontiers Grid maps no yes only implicit Feature-based yes no no Vector-based yes no no PolyMap yes yes yes 2019-07-02 PolySLAM Conclusion Conclusion: Map Formats • 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
  58. 58. 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 Johann Dichtl PolySLAM July 2, 2019 58 / 70 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 •
  59. 59. 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 Johann Dichtl PolySLAM July 2, 2019 59 / 70 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 •
  60. 60. Outlook Outlook Johann Dichtl PolySLAM July 2, 2019 60 / 70 Outlook 2019-07-02 PolySLAM Outlook •
  61. 61. 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 refinement Speed / Performance Collaborative multi-robot SLAM Johann Dichtl PolySLAM July 2, 2019 61 / 70 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 refinement Speed / Performance Collaborative multi-robot SLAM 2019-07-02 PolySLAM Outlook Outlook •
  62. 62. fin Thanks Johann Dichtl PolySLAM July 2, 2019 62 / 70 Thanks 2019-07-02 PolySLAM fin
  63. 63. Appendix Johann Dichtl PolySLAM July 2, 2019 63 / 70 2019-07-02 PolySLAM Appendix
  64. 64. Appendix Johann Dichtl PolySLAM July 2, 2019 64 / 70 2019-07-02 PolySLAM Appendix
  65. 65. Appendix Johann Dichtl PolySLAM July 2, 2019 65 / 70 2019-07-02 PolySLAM Appendix
  66. 66. Appendix Johann Dichtl PolySLAM July 2, 2019 66 / 70 2019-07-02 PolySLAM Appendix
  67. 67. Appendix Johann Dichtl PolySLAM July 2, 2019 67 / 70 2019-07-02 PolySLAM Appendix
  68. 68. Appendix Johann Dichtl PolySLAM July 2, 2019 68 / 70 2019-07-02 PolySLAM Appendix
  69. 69. Appendix Johann Dichtl PolySLAM July 2, 2019 69 / 70 2019-07-02 PolySLAM Appendix
  70. 70. Appendix Johann Dichtl PolySLAM July 2, 2019 70 / 70 2019-07-02 PolySLAM Appendix

×