Occupancy Grid Maps (Cyrill Stachniss)

so welcome everyone i want to talk today about occupancy grid maps which is a representation of the environment that a lot of mobile robots use so they use these types of map in order to represent the space around them in order to do a various of different tasks such as planning a path towards a goal location and these occupancy grid maps can be seen as kind of architectural floor plans so what they do they basically store information about which parts of the environment are occupied and which parts of the environment are free and you can see this for example in a very similar way when you want to rent an apartment the landlord typically sends you a floor plan so you have an idea how the layout of the environment is and what's written in this floor plan are typically the walls so the walls are painted with black ink for example and white represents free space and occupancy grip maps can be seen as very similar to that they store in an image based representation or it looks like an image to us the occupied and unoccupied areas in the environment so wherever we have white pixels we can represent we can see this as free space and where we have dark black pixels this could be the occupied space the difference may be that we do not only store walls we may also store all other types of objects that are standing around in our environment and this kind of gives you at least an intuitive idea on what an occupancy grid map is and in this lecture today we want to dive a little bit deeper understand the mathematical model which sits behind those occupancy grid maps and we'll discuss how we can derive such occupancy grid maps from real world sensor data so first of all why should we care about maps maps are needed for a wide range of robotics applications so whenever you have a system which should navigate through the environment and it shouldn't do that in a random fashion then robots often use some type of internal map representation those map representations are needed for tasks such as localization so in order to determine where you are given a map of the environment or for planning a path so if a robot is at a current location and wants to drive towards a target location then the robot needs to have a map of the environment in order to plan optimal or near optimal plan you may just go tell the robot drive towards the target location maybe the system will be lucky and reach this target location but if there's an obstacle in between the system may get stuck so planning approaches which rely on maps are a better idea if you want to have an effective navigation system so the the map represents the environment for the robot so all everything the robot understands about the environment is typically stored in that map and they tell us what the environment looks like this can be occupancy information so where is an obstacle where is free space this could be the location of distinct points in the environment so-called features or landmarks but this could also be something else it could also be semantic information just as what is actually there stored in the environment but this depends on our application for today we will look into occupancy information so which part of the mind is free and which part of the environment is occupied and so the task of learning those maps from sensor data is a fundamental task in most robot navigation systems because even if you provide the robot beforehand with the map the possibility that the environment changes or that something needs to be updated in the map is actually pretty high so systems need to be able to on the one inside determine if the map representation that they are using is still up to date and if not update the map of the environment and this happens typically based on sensor data so what types of fenders are we looking into i've shown here a few popular sensors that you find on a lot of mobile robots this can be monocular cameras stereo cameras or lgbt cameras or maybe laser rangefinders in 2d or in 3d and we were looking here today into sensors which provide me proximity information so this could be a laser range scanner but this could also be a stereo camera or an rgbd camera maybe not necessarily the monocular camera although there are approaches today which try to estimate the depth information also from a monocular camera so our assumption here today will be basically that we have a range sensor and we do two small examples um with that one with a sonar range sensor so something which is more a little bit outdated technology let's say that way um at least on set on robots that are out there today which typically use more high quality sensors unless it's a very cheap device um so then the second example will be a laser scanner something like this over here so we will look into the question on how to build a map of the environment with such laser scanner so in general we need to distinguish two types of map representation so called feature-based representation and volumetric map representations so feature map representations basically store the location of distinct points in the environment so there's an example here of victoria park in sydney where tree trunks could be or are one type of landmark that the system uses in order to estimate this landmark maps or map of trees and when they are you can use them this map representation in order to localize in the environment we are not looking into feature-based representations today we will look into volumetric representations this can be in 2d this type of floor plan map that we see over here or we could even extend it to 3d and then store a full volumetric model of the environment and again they are both different types of representations we can use both for example for localizing a robot in the environment given that we or we here often want to plan a path for the environment i feel that this occupancy grid-based representation are more suited because they explicitly represent free space and we can use them in order to plan a path through the free space and therefore they are preferable in several applications so today we will be looking into these type of occupancy grid maps how does a robot looks like that could build um such a map so this is in a simple example it's one of the platforms that we have in the lab a u-bot which is equipped with two of these horizontally looking 2d laser rangefinders these hokuyo rangefinders and they can drive through the environment and a very simple occupancy grip map could look like this so what you see here is basically an image rendered from such an occupancy grid maps we see white space here in the middle which represent the free space and we see those black pixels which represent occupied space so basically locations in the environment where there was an obstacle for example here was a box standing in the environment so you see the outline of that box represented in this map representation this is an example for simple occupancy grant map we can also build other types of representations so this was as a platform navigating through the catacombs of rome a platform that we had built together with other partners within a european project from rome algorithmica ke leuven and aachen and other corporation partners that we had with the goal of exploring catacombs or ancient structures and for that you will need to build a 3d model of the environment and so you can see here a part of the point cloud that the system was generating while navigating through these tunnel-like environments this is also a map of the environment a volumetric representation although you may not call a point cloud really a 3d model but we could use this 3d point cloud data in order to render for example also 3d occupancy grid map out of that this becomes also relevant for autonomous cars so we have a vehicle like this vehicle over here which is equipped with cameras and laser range scanners here on top of the vehicle we can actually drive around here on campus and then build a map of the environment so this is a 3d representation of our parking lot here in the back you could see the vehicle which was driving around here and building that map from the three lidar data while navigating through the environment we can do this completely just representing the geometric information as we have seen it over here or we could also store semantic information so also storing what we actually see for example what's the road what's a different car what are building structures here encoded in colors and we can use this from the raw point cloud data um basically coloring every endpoint depending on the semantic class and we can also turn this into a voxel map so voxel map can you can see this as an extension of this idea of this 2d floor plan into 3d where you basically have not 2d elements pixels but you have voxels so 3d elements which represent a certain part of the 3d space like like small cube and tells if this cube in the world is occupied or not this is kind of an extension of this idea of occupant cigarette maps from 2d into 3d and we want to look today into the underlying model that sits behind this occupancy grid maps all voxel maps and storing occupancy information so if we think about the typical mapping tasks then we typically want to estimate the map of the environment given my sensor data and my control commands so give me the most likely map given the sensor data and the sensor data can be camera data or my um [Music] my lighter data and we're interested in the map in the providing the map or estimating the map that can be best explained through the sensor data today we go for a slightly different variant of that we are looking into systems where we assume to know the pose of the platform so we're not necessarily interested in the odometry commands here anymore we are interviewer we are saying we kind of ignore that we assume to know already the poses where the system was in the environment this is encoded by going from u to the variable x here which is given so we want to estimate the map given our sensor data our sensor readings as well as the position of the platform in the environment so it's a simplification of the in general simultaneous localization mapping problem where we need to estimate where the platform is and build a map of the environment at the same time so here we are only interested in estimating the map we assume we already know where the platform is okay so if you think about an outside environment you maybe have a very good gps imu combination which directly provides you at least the other with post information and then you can use this post information in order to build a map of the environment that would be one example for this mapping with known poses at least if everything works well and we have a good gps signal and scale wise this is still fine indoors we may use other techniques in order to estimate where the platform is but for the lecture today we assume that to be given so we are only interested in estimating this variable m so what does the environment look like ignoring the potential post uncertainty that is involved in that process so therefore also something is called mapping with known poses so for today we're looking just into maps that can be built with given this pose information there are different ways to represent maps i said that before one is feature-based representation where the map is basically just a list of points of these feature locations that's something that we have seen for example in bundle adjustment where we uh estimate a 3d model of the environment that consists of 3d points this is a standard approach in computer vision and photogrammetry where we typically extract point features from images and try to estimate the location of these point features in the 3d space and if we observe these features multiple times we can basically reduce the uncertainty in the pose estimate of those landmark locations so landmark map in the end would just be a list of points typically stored their location and maybe their uncertainty that would what a feature map would look like and this is something which we don't want to look into today we want to look into this idea of using a grid map a discretization of the environment so grid maps or occupancy grid maps discretize the environment and represent a variable for all the discretized space and in our case this is the occupancy information so either part of the environment occupied by an obstacle yes or no that's basically the question that we want to answer for all parts of the environment and for that great maps discretize the environment into so-called cells or occupancy grid cells in our example you can see those cells as tiles on the floor so if you have a floor of made of tiles and these are whatever 30 by 30 centimeter tiles you can see all of those tiles as one cell and we store for every cell is this tile occupied or free and of course not all the environments have those tiles you can see them just as virtual tiles so just put a uniform grid over the environment and store for every grid cell if it's occupied or not and these grid cells can if you think about an image be seen at the pixel in an image just kind of as a simplification for you for understanding that because you probably have worked with images so we use this grid and this grid structure is typically rigid and every grid cell it's a uniform grid represents a certain part of the environment let's say a five by five centimeter area of the space and we want to estimate for that area is it free or occupied we want to do that not just for one cell but for all the cells in the environment and the occupancy grid maps typically make the assumption that a cell is either free or it is occupied there is no other state than being free occupied either there's an obstacle in there then it's occupied or there's no obstacle in there then it is free and they don't use any type of parametric representation of the environment so it's not like in the feature space where you may use a gaussian distribution in order to represent the location of a feature we just have this non parametric representation this grid structure where we store for every cell just is it free or occupied with a so-called binary random variable what is an issue of those grid maps they can get very large so especially if you think about 3d structures if you don't do this only in 2d but in 3d then the memory consumption can grow quite substantially in 2d it's probably still okay to store them as a map information so this may be seen as a disadvantage especially if a very sparse space then a feature-based representation may be better but they have other advantages for example that they don't rely on a specific feature detector that you would need if you build a feature map so just go a little bit into an example how does an occupancy grid map look like so this is a map of an indoor environment that's the building where i did my spent my phd time at the university of freiburg and these were different offices i was sitting somewhere over here for a long time and at part time somewhere over here and what the map here shows is the structure of the space where everything which is every black pixel represents occupied space and all the white pixels represent a free space so if you kind of zoom in here into this kind of doorway then we can see in the zoomed in view that there are cells which are black which are the other walls and also this part of the wall which is occupied and the rest of the space is free space you may see also some gray color over here this is basically space we haven't seen but we don't know what the um if the space is occupied or free but to that we will come in a second how we can represent this so we are talking about this type of representation so the pixel structure basically gives me the grid structure the discretization of the environment and we want to store for every pixel is this part of the environment free or is it occupied by something and occupancy grid maps make assumptions about the world and the next thing we want to do we want to look into those assumptions and then see how we can actually model this representation in a mathematical fashion so let's start with the first assumption assumption number one tells me that the area which corresponds to one of those grid cells is either completely free or completely occupied so there's only two states free or occupied there's no other state so it doesn't mean that my model may be unable to tell me if it is occupied or free that's not what i'm talking about i'm talking about the assumption that every every part in the environment the physical space either occupied or it is free so if you show this in a very small simple 4x4 grid structure that would look like everything which is black over here is the occupied space and whatever you see here and white is the free space so an environment where we have here let's say a wall or an object and free space everywhere else and again the assumptions either fully occupied or it is fully free that's again something which doesn't really hold in reality because we can have objects which are smaller than my grid size or we can have the situation that this good structure is not perfectly aligned with a wall for example if you have 45 degree rotated so if you have a wall which is by 45 degree rotated with respect to the virtual grid structure that you put over the environment you will have cells which are only partially occupied or partially free because the wall passes through the middle of that cell that's something that can happen and so this assumption is again just an assumption it's nothing that we can enforce in the environment okay so it's something we assume and we'll create a model based on this assumption that was the first one the second so if we have that what we can do with this assumption is that we can say every cell can be represented by a binary random variable so random variable which just has two states occupied or free and of course when we are estimating the the state of the environment there then we can have numbers between zero and one representing the probability for being occupied or being free but the the state itself so the two possible states can be represented with a binary random variable um so if we want to estimate the occupancy probability that's where the name comes from occupancy probability map occupancy grid in short this is kind of p of m i let's say this is a cell with index i then p of m i if it is one it means this cell is occupied the probability for occupancy equals to 1. that means i'm sure the cell is occupied in this example that cell mj maybe this one has a probability which goes towards zero that means i'm i'm sure that this cell is free because i know it's not occupied so probably goes to zero means not occupied and that means free in our notation over here and so for all of those cells we need to store one binary random variable in order to represent the probability distribution about if of the individual cells being occupied or being free okay so a binary random variable let's just consider one cell here for the moment it means a cell is definitely occupied it's p of mi equals to one if the cell is not occupied it's p of mi equals zero and we don't if we don't know anything about it we say oh actually we don't know it's free or occupied both happens with the same probability then we may represent this with 0.5 so we don't know anything about it and the probability of my prior for being occupied or being free is identical then i would could represent this with a probability of 0.5 and if i maybe gathered some information but the information is noisy and let's say i have a value of 0.8 i have occupancy probably of 0.8 it means i'm more certain that the cell is occupied rather than it is free so with 80 chance this cell is being occupied this is what 0.8 would mean okay um so um in terms of notation just to be very clear about this i use this small mi in the way i introduce it in the probability primer there's a probability that the cell is occupied the cell with the index i so this one takes the value of being occupied if we write it down in different mathematical form with the random um with the binary random variable and the random variables in written and capitalized letters let's say p of m i being occupied also sometimes written as p of x or the occupancy probability of that cell but we typically use just this kind of short notation but just to make sure you know what it means and of course we have also the opposite event that a cell is free so p of mi being free means pfmi being not occupied also sometimes written in this form or with the negation whatever form we are using just to make sure we are all on the same on the plate here knowing that what means occupancy probability so cell being occupied and the opposite event of it being free in terms of visualization we typically visualize black pixels with cells being occupied white pixels with cells being free and grayish pixels with values where we don't exactly know if the cell is occupied or free and the larger the probabilities or the more the value tends towards one the the darker the cell gets and the more the probability um goes towards zero the brighter the cell gets so we can the color the shading of the color tells us something how certain we are about the environment so in this example in this this gray cell it would be a occupancy probability of 75 percent that means the probability of being free is 25 so just kind of that you know the color coding of what i will be using here and what that actually means and we have a second assumption that the occupancy grid map does and this is a so-called static world assumption assumption that's something that a large number of mapping systems still do they assume while we're recording the sensor data the world doesn't change again that's clearly an assumption we cannot guarantee this our world is dynamic even if you are have a robot building a map of the room someone is walking through the room walking through a laser scan for example then the world is not static anymore while being observed although most mapping systems and also the approach we are presenting here today makes the assumption that the world is static so if you have something which is occupied it means it will be occupied it will stay occupied forever and if something is free it will stay free forever it doesn't mean that we maybe have conflict con contradicting sensor data and due to the sensor noise that we have that we say oh this cell is occupied although in reality it is free this can happen but we know that in reality the state of each cell is either being either occupied or being free there's a second assumption the third assumption that we have is that those cells are independent of each other so there's no dependency between those cells so if i know the occupancy probability or the occupancy of one cell it doesn't help me to estimate how the neighboring cell looks like again that's something we can argue if this is a assumption is valid or not the reason why we're using it because it dramatically simplifies my model if you think about man-made environments if you have a wall for example then if you have a salvage occupied you to a wall it's very likely that one of the neighboring cells is also occupied because our walls are typically large have a certain extent same holds for object that we place in the environment if you put a table into an environment and we have a cell which is occupied in the middle of the table it's more likely that the neighboring cells are actually also occupied rather than free so the neighboring information can encode something can tell me actually something so this independence is not fully justified but we use it as a simplifying assumption in here again these are assumptions these are things which do not hold in reality but i assume them to be because it makes my model easier and computationally tractable and that's the reason why i'm as a designer of this algorithm make those assumptions again those assumptions are always points to revisit when your system breaks so if your mapping system fails at some point it's a very good idea to revisit your assumptions and see if one of the wrong assumptions that you have made may have caused this the problem that you experience with your system so it's always very good to know what your assumptions are so in sum we have these three assumptions a cell is either fully occupied or fully free the world is static that means the um if something is free it will be free forever if something is occupied it will be occupied forever no one is moving through my space no one is modifying the space and the third assumption is that these individual cells are independent of each other and this has a direct consequence a direct impact on how i can model my environment so we said we have a map that consists of cells grid cells so p of m refers to the whole map in general and p of m in an index refers to an individual cell maybe we arrange them with an x and y coordinate but for reasons of simplicity i just use one index here which can index all the different cells in my in my environment so what we have if you want to talk about the occupancy map it is a probability distribution representing the whole map of the environment all the different states that we have and if we represent one representative in general it would be the joint belief so the probability distribution about the map is the joint belief about the values of the individual cells okay so this is represented by this factor over here so it's the probability of that the whole map is occupied is a property that cell 1 is occupied so two is occupied there's three is occupied and so on and so forth so the problem is i may have a very large number of those of those binary random variables in here and um what we're going to do we want to simplify this potentially very very large joint probability distribution and we do so by exploiting the assumption number three that the cells are independent of each other if the cells are independent of each other i can simplify this joint belief into a set of individual beliefs that's again something um on this uh if you think about the the independence that we have talked about the probability primer that's the point in time where we're using it so if these variables are independent of each other i can actually break them down in a product of individual probability distributions just talking about one single cell and this is something that you can see here we can simplify the probability distribution over the map as a product of the probability distributions over the individual cells and this is something that's a result of this independence assumption of this assumption number three so assumption number three leads to this simplification over here that we can actually write this down in this form okay so let's look into a concrete example where we look maybe into four cells so what you see here on this slide is a map consisting of four cells and um so p of m is a product of the individual cells so if you want to ask the question is the are all those four cells occupied so which is kind of just for illustration purposes done in this form it means is the cell representing um the random variable representing cell number one occupied times the probability that two is occupied times the probability that three is occupied times the probability that four is occupied okay so here i used kind of this m versus m in order to so capital m versus small m um in order to um to distinguish the random variable from the value itself so here i'm asking what's the probability that all those cells are actually occupied and i can directly take the occupancy probability values from my map and multiply them with each other maybe however i'm interested in a different map instance so if i say what's the probability that two of the cells are occupied especially those two ones and those two are free then this would be the probability that m1 is occupied that m2 is free that m3 is occupied that m4 is free just the product of them but since i don't store the probability of a cell being free in my map only the probability of the cell is being occupied i can now exploit the first assumption that a cell is either occupied or free so it's a binary random variable and can put m2 as free is means one minus pm equal is is occupied so i can turn this into the occupancy probability just putting the minus one over here and again here so these are one of those situations where it helps to distinguish between the random variable itself and the instance that i'm actually interested in for example or a specific configuration over here but the key thing is we have a map and this map can be broken up into cells those cells are independent of each other and so they turn into a product of individual small probability distributions and they are binary random variables so they can be either occupied or they can be free so that what we want to do that's the next thing is we want to estimate this type of map representation from data so we have specified how our mathematical model looks like it's a joint probability distribution which can be broken up into a product of individual probability distributions and they can be represented with a binary random variable that's what we exploited so far and now we want to estimate this map from data so given we have a send we have sensor data z one to t so center data recorded at different points in time and we know the position of the platform x one to t and we want to estimate the map so we want to estimate the map of the environment given sensor data and position information and i will be using here range sensors so but what we actually need is a sensor which tells me something about occupancy so which tells me something about is a cell i'm seeing free is the cell that i'm seeing occupied you could also do this with a stereo camera for example but we are using here range sensors um just to simplify things a little bit more and this joint belief given sensor data and position information can be broken up into uh this product of the probability that a certain cell is occupied or free given the sensor data and given the um the pose information so these things these guys here are binary random variables this is not a binary random variable because it contains a large number of binary random variables the whole map this is an individual cell and the individual cell is a binary random variable or represented with a binary random variable and it is a binary random variable that means we can use a technique to estimate the state of this variable and this would be a base filter so we could use a base filter to estimate the state of this binary random variable and there's a variant of the base filter the binary base filter which is optimized for estimating the probability of a binary random variable and we will use the second assumption in the derivation that i will show in a second um that we assume the state is static that means the state doesn't change over time so something which is once free will stay free something which is once occupied will stay occupied this assumption that's an assumption that we will be using in here that we don't model for example that the state of the system may change over time so we are looking into the occupancy grid map now and will now derive starting from this term a formula or an update rule how we can estimate this probability given the sensor data and given the position of information and again we will use the mathematical tools that we have learned in the previous one of the previous lectures in order to manipulate my probability distributions and derive an update rule for these type of map representations and that's what we're diving in now so we start deriving the static state binary base filter now and we start with exactly the belief about the individual cell that i have shown before and this is just kind of the product of those cells give me the probability of the map so it's totally fine for this derivation to just look into an individual cell so we're just looking into one single grid cell right now the first thing that we're going to do we are applying bayes rule so we use this equation and apply bayes rule and we are again similar to the derivation of the base filter in the last lecture we are swapping the variable zt the last observation with the verb we want to estimate this time it's not x it's not a localization problem it is my mi so the state of a cell so i'm swapping m i and z t so i need to swap them around this happens what here m i and z t are swapped compared to here and then we have the two additional terms the prior and the evidence that is results from a direct application of bayes rule using background knowledge so we have here p of z t the likelihood of the current observation given the state of that cell the old observations the old positional information times the probability that the cell is occupied or free given the same data except the last observation and down here we have the probability of the current observation given all the past observation and the position information but no map information being over here okay so i can do the next update rule and exploit the markov assumption again the markov assumption is something which says if i know the state of the world at a given point in time then what happened in the past is independent from what happened in the future something we also exploited in the derivation of the general base filter in the last lecture so that means all the old positional information all the old poles information doesn't matter if i want to estimate what i'm going to or estimate the likelihood of my observation given i know the state of the of the world so i can drop here all the old observations and all the old pose information i need to keep the current post because i need to know where the current scan is taken so this is something that i need to know about the state of the world so this equation simplifies here to p of z t given m i and x t and we can do something similar in here by saying um in order to estimate if the state is occupied or free um we can we are not interested in the future position information this is similar to the independence assumption that we also use in the derivation of the base filter by saying that um wherever i am in the future without any sensor information doesn't help me to estimate if the state is occupied a cell is occupied or free the only exception might be if i'm actually standing in that cell then i could say okay this should be free because i'm standing here with my camera or with my laser range scanner with my robot but again this is kind of an independence assumption that we are dropping here um we're assuming here so we drop this future position and just ignore x t over here and then we want to further simplify this expression over here by using the first term again and apply again base rule basically swapping that back after having exploited this independence assumption after exploiting the markov assumption so we exploited the markov assumption and then apply bayes rule for the first term again and again now swapping zt and mi so that's t and mi are again swapped so you can see here um this is the application of bayes rule for this term so we take this equation over here and put it in here and then we have this term consisting now of five elements so i could by using the markov assumption and the independence assumption i could turn this equation over here in this equation so we have the probability of a cell being occupied given the current observation in the current pose times the probability that um of an observation given the position information but not containing any map information so that's something that's probably tricky to estimate so what the probability of observing something given i know where i am but i have no idea what the world looks like probably tricky to estimate and then we have the probability of the cell being occupied or free given all the old information so it's very similar to this recursive part a recursive term that we had seen before in the recursive base filter and then we have down here the probability of a cell being occupied given i know where i am without any sensor information also probably something which may be tricky to estimate um and or maybe the position information doesn't really help me at least in general and then i have p of z t the probability of an uh observation given the past observation and the past position information and again that's something which is probably tricky to estimate because we don't have a map and we have no idea where we are so we have a sequence of observations taken at different points in time i can start estimating map from that but then i have no idea where this observation has been taken so this is the term which is probably hard to estimate um but what we can do is first using an independence assumption for this term over here what i just mentioned briefly we can say the probability of a cell being occupied even i know where the robot is but i have no idea what the robot sees or senses doesn't really help me unless the robot is standing in that cell but there's something let's ignore this so we can drop xt over here and this just turns into the probability of a cell being occupied or free in general and this basic just statistics about if i look to an environment or my assumption about the environment do i have more free cell than occupied cells this is basically what this what this prior encodes what the average if i just draw a random cell how likely is the cell to be occupied or free it's kind of this this kind of map prior so to say that we have okay so i have now my simplified equation over here but i still have a few terms which is which are hard for me to estimate something which may be tricky what i haven't exploited so far is the fact that it's a binary random variable that this variable just has two states so just occupied or free they're just two possibilities over here and i can exploit that now by doing exactly the same derivation for the opposite event so for the cell not being occupied but the probability for the cell being free so we'll get exactly the same derivation over here except wherever there's an mi there will be the negated event so not m i and the rest will stay exactly the same so just by doing the same thing for the opposite event p of not m i given the same observations i end up with exactly the same equation with the one i had here except that every mi is replaced by not mi okay so it's just kind of the event and the opposite event and now i'm doing a trick which i can do and this trick makes sense because i'm have a binary base filter so i have two possibilities in which the state can be in so i'm q computing the ratio of these two probabilities so the probability of the event divided by the probability of the opposite event and again this is because of two random variables i covered all the possibilities i kind of encoded in this ratio so i'm computing the ratio of the probability of being so being occupied divided by the probability of that cell being free estimated from sensor data and kind of you can write those two equations over here divide by divide them by each other and the attractive thing is now that there are all the terms which are independent from mi or not mi so this doesn't pop up are identical in both terms and so they can be eliminated in this ratio so for example um this term p of z t given x t pops up in both situations also this p of z t or the likelihood of the current observation given all the previous observations and the pose information is something which pops up in in both situations and thus can be um can be eliminated so this can this goes away and these two terms goes away because they pop up in both equations and so they can actually be ignored so um then this ratio simplifies by just having these three terms which survive what i can do is again just regroup those terms so i'm writing the event and the opposite event for all those equations on top of each other and since it's a product i can separate this into three different products so the product number one a product number two and the product number three and so the first term is the term that uses the current observation so these term over here are all the equations which contain the um the the current observation zt so what i'm robot is seeing right now the second term is a recursive term so it only it's related to the state estimate of that same cell but using only the old data up to the time t minus one and this is kind of the recurs related to the recursive term that i had it's just a ratio of these two recursive terms and the last term over here is just prior information what can i say about my map without having seen anything without having been there so what is my prior my assumption about occupancy in this world so to say okay so what i have i have the ratio of the cell being occupied divided by the cell being free given all the data it can it can reduce this to a product of three terms one related to the current observation one related to a recursive term and one related to prior information and what i will do now i will just introduce a new term a new abbreviation a new function that will allow me to simplify this expression over here um in theory i can still use this term over here and do recursive state estimation with it but it's i so far i haven't recovered the original probability that i want to have and this is something which is called the odds ratio or sensational odds ratio so the odds of x means p of x divided by 1 minus p of x and you know this is exactly what we had before it's the event divided by the opposite event and this is something which we refer to as odds of x so what i can do is i can multiply this this equation with 1 minus p of x and so this is multiplied on that side if i swap the sides i've written here p of x is odds x minus odds x times p of x so what i think can do is i can take out this can take this one add it to the other side and then what remains is uh p of x of course it occurs in both things so i can take it out of the equation 1 plus odds of x equals odds of x and then i can divide through this term over here divide by 1 plus odds of x so this goes down here so p of x is odds of x divided by 1 plus odds of x okay so that's interesting because what it allows me to do is it allows me to given i have the odds and the odds of something that i actually computed before turn that back into the probability with the simple equation odds divided by one plus odds or can also write in the way as a 1 divided by 1 plus 1 divided by odds of x so this is this is equivalent so what i have in the end i have an equation to turn those odds back into probabilities and also the probabilities into odds so both sides of this term are there so with those two equations i can turn odds into probabilities and probabilities into odds that's great because what i've done before i actually estimated those odds right so i have a rule to estimate those odds and now i can use those um these equations in order to turn the odds back into the original probabilities that i had so just by using this equation down here and using the rule that i had before i will end up in this update rule that the probability that i wanted to estimate about the state of a cell given all the observations and all the control commands can be written down in this form over here which is the ratio of those probabilities that i had and then everything 1 divided by 1 plus this term over here because there's a minus 1 over here okay so with this rule i can use those ratios that i can compute or that i need to specify or can compute depending on which term i'm referring to and can turn this into the occupancy probability and can estimate the current state of every grid cell given my sense observation and given my control commands so for efficiency reasons i can however take that a step further and use a slightly different form of this odds notation they've got lockouts notation where i compute the logarithm of those odds because as we'll see in a second this can substantially simplify the mathematical operation they need to do even those are kind of not too too complicated but we can make them even easier so that this occupancy grid update gets super super fast this was more important at the time when this algorithm was developed which was in the early 1980s but you can still use this today in order to very very efficiently update your maps so let's have a look on how that works so if again we go to these uh to these to this ratio and we have here the we have here an odds term here in odds term here an odds term and they are an inverse odds term and of course we could write it now as for example this term as odds of mi given z t and x t and if we now go into into this odd space first and then can be the logarithm of both terms the logarithm on that side and the logarithm of that side so just take the logarithm of both sides i will refer to this as l of the variable which is the log of the odds they've also called lockouts and the advantage of this is first the log odds i have this ratio already encoded in here and taking the logarithm of this turns this product into a sum so basically i just have three sums that i need to execute which makes this algorithm so fast so l of x is nothing else then log of the odds and by just taking the exponential of this odds i can of course revert it back to the original probabilities so i can go to this log out space and can go back to the probability space given these two equations and if i do so then i can write down the the log odds of this term interested in is the log odds of m i given that t and x t plus the log odds of the recursive term and the log odds of the prior just have the plus and minus over here depending if it was which part of the ratio it was it was the inverse or not so in the end what i need to do i have my current estimate in my in my in my grid cell that i need to store and this is the sum of three terms the recursive term so the old state of that cell before the inverse sensor model of the log odds of the inverse sensor model so what the current sensor information provides me minus the lock lock of the prior so in in very short this is kind of the new state of a cell at time t so cell i at time t lti so this refers to is my inverse sensor model so something which depends on what i'm seeing right now plus the old value that it had so it's just the value of the previous point in time minus my prior information so what i need to do if i have a new observation i just need to iterate over all cells and update every cell with this single equation so just need to add a value and subtract the value um from the the previous the current value that i have so basically we have a 2d array where i need to store the values um these these log odds values for every cell one one number one floating point operation or one double number and i just need to add something and subtract something from that cell and there's an operation which can be done very very fast can even be parallelized in an easy fashion so if we put this into an algorithmic form we can see the occupancy gridmap algorithm says okay i'm iterating over all cells and then i'm checking if the cell is in the field of view of the sensor so that's the sensor see this cell at all and if it sees the cell and can tell something about the occupancy then i'm doing my cell update and this is again just adding and subtracting a value if not i kind of keep the value it is so if i restore do that in an array i don't need to add to any update over here and that's it and i return my two dimensional array so something which is very very easy to do very easy to implement very fast to be done and i can even parallelize this so the for example summing over all cells i can easily break down my map into small chunks and then operate this uh to perform this operation um on different cpus on different threads just updating a part of the map so something which can be easily parallelized because there is no access to kind of the same data structure at least in writing fashion when i perform and parallelize this so something which can be done very very fast very efficiently and a few words about the history this is an algorithm which has been developed by moravec and alphys in the beginning of the mid 80s so it's an algorithm which is around the robotics community since quite a while and they used it for sonar sensors and to want to build occupancy grid maps with sonar sensors having a robot which knows its posts so was driving around through the environment building a map given known poses estimating which part of the environment is occupied or free and therefore is also often referred to as mapping with known poses because once one of the first algorithms this or breaking up the space into occupied and free areas um given non-post information but have a probabilistic way for integrating sensor information and so this was the core algorithm per se we now if you want to implement this on our real robot we need to specify what is our inverse sensor model and that as the name says depends on my sensor so depending on how the sensor looks like i end up having a different inverse sensor model that i need to specify for my sensor and i want to do now is provide you two examples on how such a model could look like so we start with the laser scanner example so the laser scanners are typically rather precise sensors especially if you talk here about let's say grid resolution of five by five centimeters or maybe three by three centimeters to environment in a rather fine-grained fashion which is typically totally sufficient for doing indoor navigation for example um then the sensor is typically more accurate than with grid resolution and so um if we get a measurement like a laser range measurement where the noise is typically smaller than the grid resolution um we can say okay assuming that it's not an outlier at least then we can say this cell where the beam ends is a cell where there's an object in there and this cell is occupied and all the cells where this where the array passed over those cells are cells which are more likely to be free than occupied and given that those sensors erasers laser scanners are rather precise there's not that much noise involved in this process so how does now an inverse sensor model actually looks like so we're looking here into a deviation from the prior so is it more likely to be occupied or more likely to be free with respect to my prior information so what we have here is the range reading what the sensor sees sits over here at zt so this is here in the middle so this is the sensor reading that the sensor reports and what this graph shows us is how we update the occupancy based on the sensor reading so and this is the distance from the from the sensor which sits over here up to the cell where the beam ends up the distance where it ends up so whatever lies before the return of the of the end point so everything which is in here sits in front of the sensor and caused didn't cause a reflection so this is free space so whatever is in between the sensor and the obstacle is free space right because the the sensor ray pass through those cells and so their occupancy probably should be removed because they're now more likely to be free than being occupied and this is something that i'm exploiting in here then at that cell where the beam ends the probability that it is occupied is larger because there's an object or something which reflected that laser beam so the occupancy probability will be increased so it's more likely to be occupied than it is free and whatever sits behind that cell is unknown i don't know i don't know how thick the walls are or i don't know um what's behind the wall i can't look behind a wall so i kind of keep with my prior information i have no update to be done in here so whatever is in front of this this is kind of the free space whatever is here is the occupied space and whatever is there i have no idea what it is no information about it and so this is the inverse model that we use for laser rangefinders for performing this occupancy grid mapping if we go for a sonar sensor the situation changes a little bit for an inverse sensor model of a sonar i don't have just kind of one ray like the the laser ray i have actually a cone of the sonar signal that is emitted and then some object in this cone leads to a reflection so the inverse sensor model for sonar may look like this so they are it's basically this cone structure which goes up and opens up in here and then i got a re a reflection or return of the signal at some point over here so those cells are more likely to be occupied and those cells are more likely to be freed because if there wasn't been an obstacle somewhere in between somewhere in here i should have had a return already beforehand okay so um this is kind of the sensor cone the whole cone that needs to be updated for reasons of simplicity i'm just looking here into this kind of ray so in the in the plot that is very similar to the plot for the laser scanner um that you have seen before so that would be a good illustration for laser scanner so this red line is the x-axis of the plot which now follows and of course i can put this axis going here through those individual cells not just directly through those grid cells and the um the inverse sensor model for the sonar sensors looks similar but not identical so we still have the same idea that we have somewhere over here everything which sits in front of the measured distance is free space so whatever sits here is my free space so because i'm in front of the obstacle if there would be an obstacle closer to the robot i should have had a measured distance which is shorter the return should have happened earlier then i have my measured distance set over here where um i actually got um the return signal uh from where which the measured signal was so but the sensor is very noisy and um it may lead to reflections which are um so generate signals which either closer to me or further away from me so the i'm here not as certain anymore close to the object where the return actually happens so it's not this kind of step function for the laser scanner it's kind of a smoother function going up here just represented by a set of linear functions taking values from smaller to larger values until i have a certain size a certain peak over here if i'm kind of deep enough inside the obstacle i said okay this is still this is now an obstacle so this area here represents basically the typical noise that my sensor has which is in this case typically larger than the grid resolution that we are using and then at some point time it will drop down again so this is kind of the free space over here this is again the occupied space and the information down here is again um no information that is actually provided because i can't look behind an object and again kind of how thick how many cells i'm updating here is basically a prior information an assumption about how thick my walls are and i need to have a certain thickness over here because the sensor is so noisy if i just have very very thin walls the number of times i'm actually reducing occupancy probability would typically override the points in time where i'm seeing occupied obstacle and therefore i have to generate a certain thickness over here so how does it look in reality so this is a plot of different sonar range scans taking with the b21r robot this was actually a work that i have done in my very very early years when i was starting a phd so you have let's say a map that you had built so far you see the robot over here the dark cells and the bright cells and these are different sensor readings that you get from your sonar and the sauna basically has fires different sonars at the same point in time and generates these updates for this occupancy grid map and you're basically integrating all those updates with the updated rules that have been showing in order to extend the maps you can see here this was the map before then you get this set of observations and then you can see how your map gets updated and if you drive through the environment for an extended period of time and take your sensor data then you can actually see here how a map looks like so you have this ring of sonar sensors over here with 24 sonos firing at different points in time while you're traversing through the environment and then you can actually if you have good pose information build a map which looks like this this occupancy grip you can see here some areas are more grayish this is a result that the robot wasn't certain hasn't seen this space only for a few times also here the reflection were a bit tricky also over here you can see this were actually glass panes which led to reflections and they can see that some obstacles are generated actually behind the real obstacle this is just cause due to the problems that the sonar sensor has when observing the environment then we can actually turn this map and return the maximum likelihood map so what are the cells which are the cells more likely to be occupied or more likely to be free which would then return this type of map representation over here where we just put the values to zero on one based on what we get from this sonar reading so you can use this even with very old sensing technology and even sonars which don't provide very accurate results often and have some issues such as reflections crosstalk and all these things but if you integrate multiple observations into a map representation you actually actually get decent or surprisingly good maps out of this sensor information today we will most of the time use this with laser range scanners so with them with this kind of step function model that i've presented in the beginning so that if you have sensor readings looking like this so these are kind of just the end points plot you can actually even see here that there are some sprinkles around these actually person walking through those corridors while recording the data so violating the static world assumption but if you turn this into an occupancy grid map you can actually see that those moving objects are often removed in this map because the person is moving through the environment the robot gets multiple observations so it sees a space once or twice is occupied and afterwards as free again and if you see it more often as free as occupied you basically attribute they attribute this effect to your sensor noise rather than this aesthetic world but if a person would be there and what's standing for the majority of time then just moving away once the robot sees these cells more often to be occupied rather than free then of course the cell would stay occupied so it's just kind of an effect that the static world assumption is not too bad for let's say quickly moving obstacles but in general we haven't addressed that topic of um um of dynamic dynamic objects at all in this uh representation and actually not taking it into account in the mathematical model so if you want to implement this yourself one thing you need to do is you need to estimate which cells are actually covered by a range reading and this is something which um you can do if you think about drawing a line in an image so let's say you want to draw a line from here to here an image which cells should you actually color in order to draw that line and this is an algorithm which stems from cryptographics which is much older than the occupancy group algorithm which is brzeznam's line algorithm so if you want to estimate which cells are affected by a range reading by laser scanner for example then you typically used braznam's line algorithm which they provide this coordinate and this coordinate it would tell you the number of the grids of the coordinates of the grid cells which are affected by this line or which is crossed by this line then you would need to check those cells for occupancy or update those cells it's a rather straightforward algorithm which just basically estimates always um which part of this grid cell is intersected by the line and then moves to the left right bottom and so on and you basically get a list of your cells that are being updated so if you want to implement this look for brazen ham's line algorithm if you want to do this on your own um this is an example of an occupancy grip map built with a laser scanner from a larger indoor environment this is the seaside building at mit where you can driving we're driving around with the robot recording laser range information in 2d and then building a map of the environment you can very nicely see occupied and unoccupied space you can clearly see the walls you can see some areas here where desperate standing where it's a bit more cluttered but overall you can see the building structure quite nicely and also brought you a short video how you can actually do this online so while the robot is moving through the environment and kind of explores the space we can update the map and integrate this information into the mapping process online so we're performing this recursive map updates and every time we get a new sensor of the observation and then we can build a map of the environment using this information this is kind of a standard approach how from a robot moving through the environment assuming we know the trajectory and taking the sensor data into account we can build a model of the environment and this basically brings me to the end of the chapter today as an introduction into occupancy grid mapping which is a popular technique for building maps of the environment so we have looked here only in 2d where the basic occupancy grid map algorithm was formulated in the 1980s by morovic and alphys and their key idea was to discretize the world into independent cells so the space is discretized into the uniform grid into cells and every cell represents a small part of the space and each cell can be either occupied or free so we can use a binary random variable to estimate if this part of the environment is free or occupied and we're doing this with a model that assumes independence between cells so we assume the cell is either occupied or free there have been extensions of occupancy growth maps which try to relax this assumption and take enabling information into account but then the underlying estimation algorithms become more complicated we cannot use these simple easy update rules that we have derived over here the occupant secret mapping algorithm in its original form also assumes a static world assumption so it assumes the world is static doesn't change that means a cell is either occupied or it is free and if it's occupied it will stay occupied if it's free it will stay free again an assumption which doesn't hold in general or in most situations doesn't hold in the real world but some an assumption that the mapping algorithm does and as a result of those three assumptions that i just said which are the three key assumptions which are involved in here we use a static state binary based filter for every cell to update this why are all three assumptions in there it's a static state so the word aesthetic it's a binary base filter so bind estimates a binary random variable and we do that per cell individually that means the cells are independent of each other because we just compute the product of the joint of those updates we have computed given the formula that we have presented here um and um so it's mapping with known poses mapping with non-posters is relatively easy we could easily write that down over here and so far we assume to have known poses of course the situation changes if your poses are unknown because then the uncertainty of the pose information adds an uncertainty to which cells you should update and then the whole system becomes much more complicated and this is then something we refer to as simultaneous localization and mapping so localize the robot and at the same time build a map of the environment but for today for now we assume we have post information what we then have done if we looked into this log odds notation i use the log odds representation for every grid cell just because it's so fast to compute because then everything just turns into computing sums over cells in a map something which is very easy and can be done efficiently i only have this inverse sensor model but i just need to specify it once or pre-compute it and then basically it's just taking values from lookup tables and adding them up which is very very efficient efficiently realizable on modern computers as well as outdated computers which are 30 years or 40 years old by now another advantage of the occupancy grid map was that we don't need predefined features we don't need to have a feature detector optimized to our sensor to estimate whatever sift points from images word tree trunks from laser range data so we are completely agnostic to this the only thing we assume to know is that we get range information which tell me where the next obstacle is what's the distance to the closest obstacle in a certain direction and this is the assumption that i assume to get from the scanner that i'm using over here again we have done everything in 2d we can also extend this to 3d using voxel grids or just having a grid in the 3d world where every voxel every pixel turns into a cube and just stack the cubes also in the z directions and then estimate the same random variable for the 3d world i can directly employ that the only thing i'm suffering if i do this in 3d typically is that the memory consumption grows substantially and maybe i don't have enough memory to store larger maps and so things may get a bit more complicated and you can have optimized data structures such as octrees or octre inspired implementations realizations in order to store your points so you maybe don't want to avoid estimating the whole grid as a 3d array but in theory the same what we've done in 2d can also be applied in 3d so if you want to get further information about what i have presented here again recommend the probabilistic robotics book the chapter 4.2 investigates the static state binary based filter in a very similar way that i have done this here and we have derived how we actually end up with this static state binary based filter by using the trick of computing the ratios of both probabilities and then the occupancy grid mapping algorithm is also in the book and which tells you how to update this how we do this for the sonar reading or how we do this for for the laser range scanner for example with this i'm coming to the end of this lecture and i hope you have learned today how you can build a map plan like map of the environment if you have a range scanner um you can treat all the cells individually and estimate for every cell if it's more likely to be occupied or free which then turns into a probabilistic representation if you do this in a correct way telling the probability that the cell is occupied or that it is free in order to get probabilities out of course you have to specify your sensor model and have to estimate what is the probability of a cell being occupied or being free if it's in front of the scanner this can be a bit more tricky you may need to calibrate your your specific sensor so you get meaningful probability values out of this but the mathematical framework for that is available and you can do this estimation on your own and build maps of the environment which will be also one of the homework assignments that we will give out for that course that you need to build a map of the environment given pose information in later range data and come up with the model of the environment that's it from my site thank you very much for your attention and i hope you enjoyed this lecture learning something about occupancy grid maps thank you very much

Channel: Cyrill Stachniss

Views: 9,353

Keywords: robotics, photogrammetry

Id: v-Rm9TUG9LA

Length: 69min 24sec (4164 seconds)

Published: Thu Aug 27 2020