An Incomplete Scan Matching Tutorial Giorgio Grisetti February 10, 2018 In this document we provide an explanation of the basic concepts for devel- oping a simple maximum likelihood mapper for laser data. We rst introduce the notation, then we describe how to compute an occupancy grid map, given a known set od poses. Subsequantly we present a simple gradient based scan matchgin algorithm. 1 Notation xt: robot pose at time t. | zt = (z1 {z } t ; ; zn t ) : scan taken at time t. beams ut: odometry movement which brings the robot from xt1 to xt, t. mt = ft(x; y) ! [0; 1] occupancy grid map which can be seen as a function that maps each point in the probability of being occupied. 2 The Scan Matching Problem A scan matching algorithm works in two steps: it determines the most likely pose, given the actual measurements and the previously build best map: ^xt = argmax xt (p(xtjzt; ^xt1; mt1; ut)) (1) it computes the next step map given the previously built one, the corrected pose and the range reading: p(mtjmt1; ^xt; ^zt) (2) In the following we present two simple approaches for performing these two steps. 1 

3 Frequancy Based Occupancy grids An occupancy grid is a discrete world representation. It describes the world as a matrix, whose cells represents the probability of being occupied. The cells are considered independent. Here we present a simple algorithm for updating an occupancy grid, based on a frequentist approach. For each cell of the map mx;y we keep two numbers: the number of times that cell has been visited vx;y, and the number of times that cell has been found occupied bx;y. Let mt1 be the map at the previous time step, xt the robot pose at time t t ) the reading. We rst consider the range reading translated t ; ; zn and zt = (z1 according to the current robot pose t ) = ^xt zt Each beam zi t can be described with its endpoint ^pi spanned by such a beam lies on the segment (^xt; ^pi before th endpoint are detected as free, while the endpoint is occupied. t ; ; ^zn t. The cells in the map t). All the cells wich are ^zt = (^z1 (3) The we can incrementally update the cells of the map for each beam, in the if occupied if free (4) (5) following way: mx;y t = (bx;y t ; vx;y t ) = { (bx;y t + 1; vx;y (bx;y ; vx;y t + 1) t + 1) t The probability that a cell is pccupied is p(mx;y t ) = bx;y t vx;y t 2 

4 A simple gradient descent scan matcher Here we describe the simple scamnmatcher you nd in this software bundle. The basic principle of this algorithm is to perform a gradinent descent search, on a score function s(x; z; m) of the pose, th reading and the map. The overall algorithm works as follows: bestP ose = xinit bestScore = s(bestP ose; z; m) searchStep = initialSearchStep iterations = 0 while (!iterations < maxIterations) { maxM oveScore = bestScore { bestM oveP ose = bestP ose { for move in (Backward; F orward; Lef t; Right; RotateLef t; RotatrRight) testP ose = computeP ose(bestP ose; move) score = s(testP ose; z; m) if (maxM oveScore < score) maxM oveScore = score bestM oveP ose = testP ose { if (bestScore < maxM oveScore) bestScore < maxM oveScore bestP ose = bestM oveP ose { else searchStep = searchStep=2 ∑ iterations + + s(x; z; m) = i s(x; zi; m) Here a critical issue is how to compute the score function. A simple approach is to consider a score function as the sum of the score of the single beams score. (6) Our choice is to use a simple endpoint based model. Given a pose x and a reading we consider individually the single beams zi. We compute the cell of the map in which the beam endpoint falls. This can be done by translating the endpoint according to x as ^zi = x zi We compute the cell of the map in which th endpoint falls. We found the busy cell ^mx;y in the map which is closest to ^zi. We compute the score as a function which decreases with the increase of the distance among ^zi and (x; y)T . The squared distance d2 can be computed as d2 = (^zi (x; y)T ))T (^zi (x; y)T )) s(x; zi; m) = ed2= (7) As an additional optimization for each map cell mx;y we can consider as its center the center of mass of the points falling in that cell. 3