Graph-based SLAM using Pose Graphs (Cyrill Stachniss)

hello I want to talk today about graph based simultaneous localization mapping on graph based slam and introduce the so called least squares approach to the slam problem using something which is often referred to as post graphs so this is a popular technique which you find today in most state-of-the-art systems out there that are designed to build a map of the environment and localize a platform within that map and we're looking here to a special form of that which is the so called post graph based approach that is an approach which doesn't use landmarks but reduces everything to the post so everything is kind of reduced to the poses or landmarks are marginalized out this leads to leads to graph structure which is actually rather easy to to maintain and is actually very flexible also in terms of the observations that you're including and therefore became one of the standard tools when doing light abasement for example or working with RGB d cameras or similar so most of those systems which build maps of the environment which have a certain size which are not let's say a single room or something like this typically use a graph based off postcard based so I'm going to approach in the background in order to correct the poses and those are consistent map of the environment and the goal of this lecture today is to introduce you to these graph based slam systems so that you are in the end able to build your own system that performs the optimizations in here and what we're using you know to do this is the technique all these squares that we just have introduced in the previous lecture and so I assume you know how these squares work and at least in a very basic setup so those Newton is something that you should be able to explain or implement and with this tool at hand and with knowing how ICP works what is what the output of ICP is you should be able to understand this lecture today so within the typical slam programs we are here in the graph based approach which is as I said over the last years became the most popular technique and basically is a least squares approach to slam and of course their extensions using different error functions for busty five kernels and things like this but we are sticking here with the basics which should allow you to understand how this grave bus graph base design actually works so least-squares again was an approach for computing a solution of an over determined system over determined means here that we have more equations than unknowns unknowns in our example our parameters where the robot is in the environment which will generate us this post graph and the equations stem from our constraints and constraints can result from observations or from control commands and our goal is to find the configuration for those poses for that post graph that minimizes the sum of the squared errors that are introduced by those equations survive the observations so that we have a configuration of these parameters of these nodes in our graph that allows us to estimate where the robot is so that the current configuration is as good as possible in line with what we have seen or experienced and again this least squares approach we apply today to the slam problem so let me very easily introduce this problem so consider we have a mobile robot like this guy here driving around through the environment and at distinct points in time let's say every second for example we are generating a pose and the poses here explained or illustrated by those triangles so the robot move through the environment and while it is moving through the environment let's say every second or whenever the robot moved more than three meters or five meters and it doesn't really matter here we create a pose or variable and this variable describes the pose of the platform at that point in time pose refers here to the XY or XYZ location plus the heading information so XY and the orientation theta in the 2d case or the operational angles for example in the three-dimensional case and while the robot is driving through the environment creates these poses in this so-called post graph and between those poses they are constraints and these constraints result from the motion the platform and so for example you have a robot which counts revolution of the wheels let's say it counts often have the wheels been turned and this allows the platform to estimate it's supposes so let's say I'm the robot right now here I'm creating a note at the quarter point of time so I'm driving around I'm turning my wheels turning my wheels for a certain amount of time and I'm standing here and creating a new pose saying a new variable and then there's a exists a constraint between the corn post where I am and the post where I have been before and this constraint results on how many revolutions do the wheels have did the wheels made and so if there were because the revolution of the wheels are related to the distance for example between those two nodes so this allows you to say okay post two is for example a meter away from post one you're typically uncertain about those about this information because maybe it's a meter and to 30 meters or 95 centimeters or a meter 20 or a little bit more to the right or to the left so you see there are some uncertainties because this information is not perfect so see those constraints is kind of soft constraint which say okay post two should be approximately immediate weight from post number one I think you can have get the information so the robot drives around creates posters after different points in time after things has traveled a certain distance and then we have those constraints and those constraints can stem from the odometry information or the robot may even use the sensor for example scan matching or ICP aligning consecutive scans and in order to estimated suppose so this can generate this type of structure that we see here and now the robot continues moving through the environment at some point in time it revisits a previously seen place so it comes back to the same area over here and this is one key element in the context of the slam problem something we often refer to as loop closing so there what comes back to a previously seen place along its trajectory and at that point in time it can actually not create only these sequential constraints but can also generate those constraints here to previous poses how does that work so this can work for example with through an ICP approach we have introduced ICP before where we take for example 3d laser scans of a platform taken at previous point I'm so taken here or here and align them with the scan taken from this location and through the alignment of the skins we can actually compute the relative transformation between those nodes something I was in last lecture on the ICP lecture referring to as virtual constraints or virtual observations and this I exactly those edges over here are those constraint soft constraints over here so by revisiting previously visited or places where we've been before or reabsorbing previously seen areas we can generate these constraints between non successive poses this kind of loop closure so to say and so while the robots driving through the environment we are getting this kind of graph structure so it's not a kind of chain of poses or a list as was before it's really grass structure that we now get here and this is what gave the name post graph because we have a graph that consists of process of these triangles and so this graph is very central to the slime problem that we also call it the graph based slime problem and we use it to represent our problem here so this type of graph is the representation for our slam system so everything in the graph that we have in there is important for the slam problem we will use this information in order to compute a solution so the graph consists of two things nodes and edges every node in this graph represents the pose of the robot at a certain point in time again it's typically a three or a six dimensional vector depending if we are modeling robot in the 2d space or in 3d and then we have the edges and the address are kind of the relations between those poses so they tell me oh this pose should be right it to the other post in that certain way and this information can come from odometry information more from my sensor observation for example performing scan matching so I can say the pose number ten is related to post number eleven by being one meter away and at the same point in time you can say the pose number ten can also be related to post number twenty in a certain distance and so this information is then being used in the optimization problem to formulate these equations that are used for the constraints so every edge is connected to two poses and relate those two poses either coming from odometry information or from my sense observation such as ICP and in graph-based them we are building up that graph so we build up this full graph of poses and constraints between poses and then the goal is to change the location of the poses so we're basically shifting and rotating those poses in the post graph around so that we minimize the overall error or the squared error that is introduced by those constraints so those constraints can be conflicting for example if I am here and saying the next post should be one meter away from that pose and then I'm driving around revisiting the environment and come back to a previously seen location and then based on this observation which comes from this from from relating those poses this post should be shifted a little bit okay and then I have a constraint constraint which says they should be a meter away and the other one should be shifted in a certain way and this leads to effect that those constraints are conflicting and I want to find the configuration of the nodes that minimizes the errors introduced by the constraints so we just have also an example here how that looks in reality so if we have a robot driving through the environment so this was a map built by perfect rough and what you see here is a map built from the laser scanner where three laser scanner moving through the environment and this is just encodes the odometry information so just estimating how the robot move based on the revolution of the wheels and you can see actually this map is extremely messy you can't even deal with that so you can see these are always the same rooms mapped at different locations so odometry itself is typically not sufficient to build a good map but what we can do now we can actually generate this post graph in this map where we have been and also relate their all those constraints with each other so we get something like this so you see those lines over here which represent the poses and also represent the constraints so that since all this poles should actually be here this boat should actually be over here this pole should actually be over there and so these are those constraints and we now have a graph of all these constraints and now we want to find a new configuration of those nodes so that the error is minimized so that those constraints are actually fulfilled as good as possible so we can in that point in time actually forget the map which just was deeply depicted in the background and we are only interested in actually finding new configuration form of those nodes because those nodes corresponds to the poses and once we have the poses we can actually build a map just based on the corrected post information again mapping was easy once we know the poses so what the graph based approach does it takes this thing here is an input this graph structure and performs a least squares minimization so that it finds new configurations of the poses that minimize the errors introduced by the constraints and this will lead then to map that looks like this where all the trajectories are kind of not overlaid with each other and the quadratic error introduced by those constraints is minimized so that when I take this pose information and then render a map just performing basically mapping with non pulsus building an occupancy grip map I will actually get an information which looks like this and this is now a consistent map of the environment so you can see here also this room is always at the same place and not spread all all over the place and this process of turning this graph into this graph this is what we're talking about the following the error minimization so to correct the trajectory of the platform that the man can use this trajectory to build a map of the environment environment okay so this is I have to tell you that this is only one part of this line problem this is what we typically call the back end or the optimization part of that in reality of course there is more involved in this available real world slam system because you need to turn your observations into constraints and this is something we typically refer to as a front end so taking the raw sensor data and turning the sensor data into constraints is kind of the front end and taking those constraints and optimizing the graph in returning the corrected poses is what's often referred to the back end in reality this is an interplay between those two so the front end relates both with each other then it being optimized then this optimized map information is used to make new data stations so in reality there's actually interplay between these front-end and back-end thing but here we are only looking into the backend parts into the optimization part and you can envision the front-end part basically as the ICP algorithm that we've introduced ICP is typically used for laser based slam systems as a front-end to generate those constraints and the constraint is here actually the relative transformation so the outcome of the ICP algorithm this rotation matrix and this translation vector and they are basically forming a constraint together with some uncertainty information and there's a little bit more to that than just vanilla ICP in order to do that well but for the moment you can imagine we are running an ICP based on a laser scanner for example which gives us the relative transformation between poses this is kind of this front and part here and we are using this raw data or this constraint data in order to build the graph and or to represent the graph and optimize that graph okay so the RAF consists of nodes so the individual triangles here this blue triangles every other triangles represents one node X 1 X 2 X 3 so this X 1 X 2 X 3 X 4 X 5 and so on and so the overall graph represents all n nodes prepares X and this simply be all all nodes put together or stacked on top of each other so each X I is the pose of the robot at time T I so at certain times them so assumed to have unique time stamps being here and if we have a constraint between two poses from a node X I to node X J then this is a constraint this can result from a dhama tree information or from this virtual observations and these are those edges over here and we will now look a little bit into that how we actually describe those edges and then once we have those edges and can interpret them then we see how we can actually perform the error minimization tasks using the idea of least squares or in this case girls knew okay so an adjectives between two nodes if the robot moved from xi2 xin so there's now a consecutive action this is an edge which can be generated from odometry war also from incremental look ICP so matching the current skin against the next skin but for simplicity reason just assume right now it's odometry so just by counting the revolution of the wheels so we are here the robot is currently here it moves forward and then based on the odometry information it can estimate where it should be given the robot or the motion follows the kinematic model that the robot has about itself and this is basically the information what I like the motion model that we're using so if you think about the previous lectures this is basically counting the translational velocity of the rotational velocity based on your steering such as differential drive or a command steering and that tells you how much you should have moved if everything looks good and for a probabilistic point of view that's basically a mean estimate of where you have been moving within the time difference from i to i plus 1 and this to become some odometer information so this is a transformation which would have to be which tells us how X I can be seen for how X I plus 1 can be seen from X I so see this is a transformation so how do you turn to transfer one port in the other post so all relative post information an edge can also exist between two arbitrary pulses so that I and I plus one but I and J where J is different from I plus 1 and then the we typically need to relate this information with each other using the sensor data so as an example so consider this know as a 2d illustration so we have our robot at X I sitting over here and this is the scene it is observing with a laser scanner and then at a position X J it's here and it's observing this part so by relating these black line and this blue line treating this as a scan and overlaying them with each we can actually generate kind of a virtual observation we're saying we can have an idea where XJ should be as seen from expire although we can't really see XJ we can't really see where the robot is in the future but by relating it to the environment that we are seeing from both locations we can actually generate something which we call a virtual measurement it's it's nothing else than the result of the iterative closest point algorithm in general so again this we see this kind of a treatment or a reference frame and by matching the black line and the blue line we are basically moving the blue one over here and then if we know where the blue one is we also know where X is in this reference frame of XJ so by constructing this kind of virtual measurement through an alignment procedure such as such as ICP or if you think about vision this could be computing the relative orientation for example using a five-point algorithm or the eight-point algorithm for image features but we are restricting ourselves here for the simplicity to ICP then we would get this type of alignment so you see here the black and the blue lines are live at top of each other and this allows us to relate the poses xixj with each other and then this transformation from X I to XJ can then be seen as a virtual measurement about the position X J as it would be seen from X I so if X I could see the robot at XJ it would be the pose of the robot relative to its own post but as we can physically not see that we do this through the relation of the environment and again this is a full transformation between those two poses so it's not just kind of distance vector or something like this the full transformation liquid became transformed the post X I to the poles X J or the other way around so how do we encode those transformations we typically use things like homogeneous coordinates for doing this or they other ways in which you can do this but at least for formulating it and writing it down in here I will be using homogeneous coordinates because this is very easy to do so just as a reminder homogeneous coordinates allow you to express what chain transformations like translation and rotation in a very elegant way so and then everything which is here written and capitalized characters you should see as a transformation so this is kind of the transformation basically of the poles X I I'm seen as a translation and a rotation in a global reference frame so so this is a homogeneous matrix this is also homogeneous matrix representing this suppose X I XJ and if I have those matrices I can here invert the matrix X I and multiply the matrix X I plus 1 to it then this is relative transformation where is X I plus 1 as seen from X I and the same holds here X I inverse X J means the note how the node J sees the how the node i sees no J so how J would be seen from node I so just as a very short reminder in case you have forgotten about homogeneous coordinates just a few words how what that is so much news coordinates are a different coordinate system or coordinates which are typically used in projective geometry it has some nice advantages of being able for example to express points which are infinitively far away a lot of things but that's not kind of the reason why we are using it here in the lecture we're using it here in a lecture because in this homogeneous coordinates a single matrix can represent all fine or even projective transformations and that's attractive for us because we will be using here rotations and translations so rigid body transformations which are subset of those two transformations and through matrices and so that means we have a matrix that represents a translation and we have a matrix which represents rotation and you can just multiply those matrices in order to change the transformation after each other and that makes it very elegant in order to write it down and this is one of the advantages of homogeneous coordinates that they typically simplify the mass at least for us when we need to write that down so doesn't work I just have a short example and if you need more information about this I recommend you to watch my lecture on homogeneous coordinates but just as a very short reminder if you have want to represent the n-dimensional space like the three-dimensional space for us then homogeneous coordinates use one dimension more so n plus one dimensional space so for us it would be four dimensions for modelling in the three dimensional space and the vector in homogeneous coordinates let's say X Y that is extended by a new dimension X Y that and the next dimension which is one in this example there's a mapping from the occluding space to homogeneous coordinates there's also backward mapping which we say we have the four coordinates XY z-- that W in this example and then we take the last dimension and divide all other dimensions by that last dimension and then drop the last one so we go from a four dimensional vector to a three dimensional vector dividing all the elements by W and so it's kind of a normal form of normalization and if we do this this forward backward mapping then we can actually express translations and rotations elegant way so a translation can then be expressed in a four dimensional matrix which has ones on the main diagonal here everything is zero and those three dimensions are the translation X translation Y and translation in that and the rotation can be expressed by a 3d rotation matrix the standard rotation matrix as you know them here in this upper 3x3 block and so this is a zero vector and this is also zero vector and the one and so by having this rotation matrix no genius coordinates in this translation in Eugenie's queries we now can multiply this translation matrix and the rotation matrix with each other and then have a translation and rotation expressed in one single matrix that's something you cannot do in the Euclidean space and but it's very very elegant for writing this down and chaining those transformations so if you go back for example something like this to express this in homogeneity in including coordinates is much more difficult and here it's very easy to see you take the pulse affects the transformation which encodes X I and you basically invert it so you can just by inverting this 4x4 matrix you are actually inverting the transformation and multiplying to that from the right-hand side XJ so it you can see then that this is actually the node X I as it would be seen from the node XJ and so that's what we are using here in addition to this our constraints are affected by noise because they are we have observations so for example of dhama tree is typically less accurate than using your sensors information if you think about laser scanner for example but even your laser scanner data is affected by noise and so you the transformation that you're actually computing are not perfect and the information matrix Omega IJ is the information matrix related to the constraint between node I and the node J and encodes the uncertainty that stems from the observation so how much do we trust that information and we may have a sense of where this is the same for all constraints but we may have different types of sensors or different situations in which the sensor is more accurate in a certain set of and less accurate another set up and we can actually use this information so the higher the value the larger the values in this information matrix the more information we have the smaller the uncertainty and the bigger it is this Omega or the values inside Omega the more this edge matters in the optimization so if you remember back the least squares hard work then this information matrix is basically a weighting factor for your error so if you have a constraint which has an information matrix which is larger images it's a has a value which is twice as large it will count more twice as much inside the optimization so this edge information is actually important in order to take the uncertainty into the account and so there are them questions how would you expect this matrix to look like if you compare scan measuring done with ICP to odometry information so which should be is more accurate or less accurate and that's one relevant information you should able to figure be able to figure out the other thing which it would be relevant if you think about ICP and you make this skin alignment and you have a robot which drives along a very very long corridor and this corridor is let's say very sparse of features it's just long walls how what this information matrix look like and thinking about those questions allows you a little bit to understand if you can relate your observations and the information you can extract from the observations with this graph construction here so the first setup relate to the first question your sends our information at least from a laser range scanners typically better than the odometry information that you have again it depends how much engineering you put into your odometry information or your laser scan information and what you know about the environment but at least for a lot of that's a standard indoor mapping systems you will actually trust your laser scanner quite a bit more so you expect the covariance matrices to be smaller that means the values and the information matrices to be bigger and this may be different if you have less accurate scanner or you have a lot of motion or movement dynamics in the environment but put a lot of engineering into getting your odometry information right maybe having redundant systems taking out the Regals into account a very good dynamics model for example in case of an autonomous vehicle autonomous car you can invest quite some brain power into your odometry system that you can entrust odometry also more regarding the second question how would these information matrix look like or the corresponding covariance matrix look like in a long featureless corridor so what does mean a long featureless corridor means the robot can localize very precisely with respect to its right and left inside because you see the walls of the corridor so you have a very small uncertainty with respect to your motion to the right and left inside authoress respect to your orientation giving that you have very long walls it could to be a line the scan very good with respect to a corridor so you shouldn't get much drift to the left and right inside but what you're actually is hard to estimate is where you're moving along that corridor so you have a very high uncertainty along the main axis of the corridor so you would expect to get in covariance matrix which is very narrow to your right inside but very long and stretched along the main axis of your corridor and so based but if you inspect your information matrices or covariance matrices of your graph you can actually see something about what this what information is the center actually tell you and this then is an important information how then certainly actually propagates in the end through your graph clearly squares problem because if you have very large uncertainties along certain dimensions this will also lead to a large uncertainty along this line okay so let's have a look to the post graph and look to all the information that we have so what we see here is a very very tiny post graph we just have kind of two nodes over here xixj so you can see this basically is cutting away everything else and what we have is we have a current configuration of the nodes so and this is those two positions here just let's say in the 2d plane just to make it easy to illustrate that here so this is we think this this node sits over here and that no it sits over there and there's a current configuration of the graph now we haven't constrained between those two because there's a common observation for example from ICP and let's say this is what the observation looks like with the corresponding uncertainty here illustrated through a covariance matrix so this is basically this is the position where if you if I would sit in X I would expect extry to be you can see here that there's actually a mismatch between what the observation tells me and what the current graph configuration tells me so this is an error that we are computing and this is the error that we actually want to minimize okay so this thing should be brought to zero over all constraints of course so and this edge here is basically represented by the kind of the mean observation he encoded as that I J and an information matrix which is here illustrators who the inverse of it through the covariance matrix and so this is the information we store in every graph so every cut every observation will lead to such an edge and what we're actually trying to minimize we want to shift the configuration of those nodes in order to minimize this error so in this case it means XJ should actually move here in order to bring this error towards the euro and of course the problem is it's not just one edge which is involved in this otherwise it would be easy to satisfy this constraint we have the goal of doing this for all our constraints so this was just kind of one example but of course the whole whole graph consists of those constraints and what we want to do is we want to minimize the over some of that and this is where the least squares approach or the ideas of least squares actually kick in so we can go back and to our previous lecture and think okay this is now our squared error function we have our error vector relating the constraint IJ with each other the information matrix and again the error term or we can kind of rewritten this in a short version where we just put the backdoor X in here the whole state vector so this was the way we were formulating this before in the previous lecture where we say the error or the the error vector here depends on the state variable X what is important now or the difference here in this land problem we can informal ate it in that way that we say not the whole vector X depends actually on this error term but only X I and XJ that means only the node X I and not XJ so those to contribute to this individual error term and all the other state variables do not matter for that single constraint and this is something an information that we will later on exploit in order to solve the same problem in a very efficient way or in an efficient way because we know that the individual error terms only depend on a very small number of variables and that's something that we will exploit later on the first question we may look like what does the state vector actually look like so even I want to sit down and actually implement our system how should that actually look like and the and what we need is our our state vector X is simply the concatenation of the poses of the graph so every vector from one vector for every node of the graph and we just kind of concatenate them with each other it gets a very very very long length of vector and it's always kind of ex-wife theta ex-wife theta X Y Theta is always the X of the first node the Y of the first node the orientation that the robot had when being there and then for the second node X Y theta X Y we go or if you're doing a three dimensional world X Y that you're patrol for example or another representation on how we could represent the the sixth dimension degree of three impulses okay so we have that we know what the state where it looks like the next thing is let's look into the error function how do we relate those error function with each other we said yes of course those the individual poses Xin XJ should pop up and also the observation should be take into account so the result of the ICP approach should play a role in here and we can formulate a single constraint now in the following way so what do we see here so let's just look only first to this part over here so the first thing so if X I inverse X J so this is kind of how X I sees XJ or how XJ is seen if I would if the center of the coordinate system would be in X I so you can think of seeing yourself standing in X I and then see where XJ actually looks like so this is what this information provides and it's directly very easily computed through the homogeneous coordinates and then we have an observation as observation zet tells me how X I sees XJ this is a result of the ICP algorithm so the transformation that results from ICP so the rotation matrix and translation vector this is this that and then an invert was that and multiplied it here from the to the configuration of the graph so the whole block here you can see where is what the difference between the pulses with respect to the graph and what's the difference of the pulses with respect to the measurement and so one is kind of you can see this as kind of the forward transformation on what the graph tells me and then a backward transformation on what's the observation tells me and then I need to look into the difference between where I started and where I end up and this is basically my arrow function okay so that what actually happens in this part over here so the relative transformation according to the graph in one direction and then the inverse of the of the observations are turning this around and going back in order to compute the difference between them and then there's a function called T to V over here magic function T to V means transformation to vector and V 2 T means vector to transformation so this is basically just a mapping between a vectorized representation vector so X Y Theta for example and the transformation are expressed in moodiness coordinates such as kind of a forward backward mapping from this 3 by 3 or from this from from this vector whatever 3 by 3 homogeneous matrix into a 3 dimensional vector with X Y theta so that the transformation consisting of two dimensional translation vector and one rotation angle encoded in a three by three matrix is put into a vector and because this year again is a transformation matrix and we need to put it in vector form and under put into all these squares problem and that's what this t2v stands for so this gives us here in this example if you are doing this in a in the 2d plane a three-dimensional vector with X Y and one orientation constraint all we can express this also not just for a single constraint but we could express the same also for the whole state vector and then of course also only only those components matter where there is actually an involvement and it should also be cleared what happens when is the error of 0 and does the error take a value of 0 and this is exactly the case if my observation is the same then my relative rough configuration so if what the graph tells is exactly the same what my observation tells me then this equation holds if I would multiply the and then you can actually see that if I'm with multiply here is that inverse from left hand side so the inverse observation and this term is the same then this is the identity matrix so that means if the transformation of going the transformation through the graph and backwards through the inverse of the observation gives me the identity matrix means there's no transformation mismatch and this will then give you the identity matrix would be mapped through this function to 0 0 0 so 0 error vector okay so the only thing we need to specify is how do we get that and this is something that our ICP that's a result of the ICP algorithm just put into a homogeneous vector and these are the posters in my post graph expressed in homogeneous coordinates and with this we actually can look okay let's rebuild gas Newton how did the overall error minimization procedure would look like so the first thing we did do define the error function that's something that we just did what we then need to do we need to linearize our error function just because we need to perform local linearization and then the leads to the quadratic form when we compute the first derivative and set the first derivative to 0 this leads us gives us a system of linear equations we solve the system of linear equations we get our the update of our state and then we iterate this process restarting here the linearization until we have converged as we discussed it in the lecture before so just as a short reminder this was our error function the original error function which we now need to linearize and again if you think about this homogeneous transformation matrices you may remember in a rotation matrix there sine and cosine functions popping up these are nonlinear functions so you need to perform linearization in linearize that thing and that's where you need to sit down with a piece of paper and actually try to perform this linearization or if you use another error minimization framework you may some systems to compute this automatically or numerically for you depending on how good your tools are but otherwise you have to do it by that means you need to compute matrix this is your Jacobian matrix and every element of the Jacobian matrix consists of the partial derivatives of deriving the your error function or the individual dimensions of your error functions with respect to the individual dimensions of your state vector R so you derive it in this case with respect to X 1 X 2 X 3 and so on and so forth and here we have to look a little bit into the details because how that looks like actually has a very big impact on how efficiently we can actually solve the state estimation problem later on so the first question we need to answer thus the error term depend on all state variables so again this is an error term e IJ that means for one single observation and thus is depend on all state variables so I'm a whole state vector it clearly depends on the state vector but it does it depend on all elements in there and within the same problem this is not the case because we said that this error term only relates the two variables to which this edge is connected in our graph so there are just two poses name of the post I hate middle index I and the post with index J on which this actually depends on so know this depends only on X I XJ and this will play an important role because this has an impact on the structure of the Jacobian that is involved in this you may think about this if you have a function which depends only on a small subset of variables if you then derive this function with respect to the variables then it will have a lot of 0 elements namely where if a function doesn't depend on one variable any derive with respect to this variable it will be 0 and this is the key inside because this leads to the fact that the Jacobian will actually be 0 nearly everywhere except for the elements which are related to the variable X I and with respect to the variables J so if you think of you have whatever 10,000 dimensional state vector and you have a constraint just between two poses then only those two poses will be related with each other and the all other 10,000 minus six dimensions will actually be zero so your Jacobian will be a larger vector which consists only of zeros and just has a very small number of elements betrayin involved in you so the partial derivative in the this Jacobian of the error function with respect to all the parameters is basically of you have here all your error vectors and your state variables will be 0 everywhere except of those blocks which are related to X I and those which relate to X J so in this case for example if X I has been to deplane its X Y and theta there will be three elements here which are nonzero three elements which are here that along there and the rest will be completely zero so we just have two blocks of variables here call them a IJ b IJ which are non zero and all the rest is zero and this is kind of a key inside because it leads to a sparse Jacobian which will also have a big effect on our H matrix later so Jacobian and sparsity if our error function depends only on the verbis 8ix xixj that means all other elements which are X 1 X 2 X 3 X 4 X 5 and so on which are not I and J are 0 and these are illustrated by these blue blocks over here so these are all the blocks which are 0 and we just have these small blocks which are non zero and this is kind of a key inside in here that we have this huge Jacobian everything is 0 in this Jacobian except those two blocks and this is the key inside so as one of the most important slides in this slide set they're having this insight because this allows you to solve it's a real world slam problems in an efficient manner why is it the case why is this so important this can be Illustrated by the fact that the jacobian has a key role for computing the elements B and H which I need to compute in order to solve my system of linear equations so my vector B and my Atrix H consists of elements which involve the Jacobian so for the H matrix very clearly you can see the Jacobian popping up twice here and also for the B matrix you see the Jacobian being over here so as we will see the sparse matrix in the Jacobian will also lead to a sparse matrix H and this is one of the key reasons why we can actually solve them in an efficient manner and I want to illustrate that now to you through a small illustration so what you have here is this vector B so the vector B is a vector and whatever thing is which is shown in blue is our 0 elements everything which is shown in red on non zero elements so I have this I have this vector here so the vector B is this vector and it consists of my Jacobian transposed so now it's a very long so these are the dimensions of my of my state vector this is my J multiplied where as an information matrix in this case for example a 3x3 information matrix and then my three dimensional error vector and this will lead by multiplying this to an error vector which is zero everywhere and has only nonzero elements at the blocks referring to X I and the blocks XJ so for one constraint this vector B is actually sparse very something very similar holds for my H matrix so and my H matrix I see my Jacobian transposed here my job my other Jacobian here my information matrix and I can see by multiplying those with each other I will get a big matrix which is basically zero everywhere except of four blocks and these are the blocks AI jajay IJ ji so these are the elements so those are identical and those all three are different on just those two are identical and and these are all the zero blocks okay and this is for one constraint so one constraint filled with H matrix at 48 for different blocks and the B vector at two different blocks so only at AI jajay IJ ji this matrix will be filled but this is for one single constraint and now we need to see what happens if we actually sum those elements up so some H up and some B up over all constraints that we have so if you take multiple of those vectors together we will actually see that we get fully dense vector B so all elements of B will be filled because all the nodes will have some constraints and so the whole vector will actually fill up its are different for my H matrix so again I have N squared elements in here and always for for every constraint will be filled so by adding all of them up I will typically the matrix which look like this so I have direct constraints here on the diagonal which are the constraint and also direct off diagonal blocks because it relates how the current post and expose are connected with each other and I have a couple of elements in the off diagonal diagonal elements which are basically my loop closure constraints so where a node X I is connected to a node X J which are not sequential and so this matrix can be seen as kind of an adjacency matrix of that graph which tells you what the connections in your graph actually are and the key thing is this matrix is typically not dense it's typically a very sparse matrix to be the larger the environment even the sparse of this matrix gets quite clear from the intuition point of view if you have for example of robot navigating around inspecting a big environment with hundreds of different rooms inside the room you only see the room you never see another room from inside that room maybe from the corridor you can see into a few rooms and and so the the all the blocks the places in the environment are very decoupled and can only be seen from the same places and not from all all places can't see all other places and therefore this leads to a very sparse matrix and this is one of the reasons that this matrix is sparse why we can actually solve and even potentially very very large linear system in the end in an efficient way so the key thing in here is that the each edge corresponds to this linear system and for the vector B it looks like this that we have two blocks which are nonzero but by adding all of them up in the end this will lead to to a dense vector but we need to build it up by for every constraint adding in two places on two blocks of this vector new elements and for the H matrix it's the same thing I have this eight matrix everything is zero they're just four blocks which I need to fill in this matrix X I and in this matrix at the positions I I JJ I J and J I and so this matrix H will be a sparse matrix so it makes sense to also represent it as a sparse matrix and depending how you do this you can either use a mathematical tool box which already provides you with sparse matrices and then you don't really need to care about it or you have to implement it yourself and then you need to make sure that you fill the corresponding elements in this block so again the key thing is an edge IJ contributes only to the eyes in the J's block of B and the blocks AI jajay IJ j I of H which leads to an overall sparse system and we can then use actually sparse solvers to do this so there's a very enough Tresca decomposition called spots to ask a decomposition which is explicitly designed for sparse matrices and it's much much faster than the standard SK decomposition and also holds for other techniques their optimizations of most of the algorithms for sparse matrices because a number of operations which need to be done it's much smaller and can be done in a much more efficient way and this is kind of the reason why we are all able to solve this systems in a reasonable amount of time so you can also if you want to build up these matrices on your own again you have your state vectors of your increments you have your coefficient vectors of your B's and then your normal equation matrix will consist of these small blocks and now we have kind of this harbor our here to represent the individual blocks of that system which is built up and then you just need to make sure you feel for every constraint the right blocks of your linear system so what you would do if you built your linear system manually your hand by hand you for each constraint you compute this error vector for each constraint you compute the Jacobian you write of course you don't compute Jacobian everywhere only in the blocks where you know it is nonzero so you only need to compute this a IJ and bij the rest you don't have to touch because you know it is zero anyhow and then you compute your for B and for your vector B the increments and add them up at the corresponding correct locations bi and BJ so these are the corresponding blocks in bi and BJ we need to add these small blocks on top of it and the same holds for your normal equations so only compute those small blocks so these are all low dimensional blocks is us this small III jji jji blocks and you just need to add those small blocks to your to your matrix so that means this is not an an addition of a whole large matrix it's just these small blocks that you need to add at the correct locations so if you do this if you build up the system on your own you need to make sure you add the constraint that the correct positions in your matrix but once you have that you have your matrix actually built up and then you can run your optimized function which then turns out to be very easy say while you're not converged and the convergence can be whatever the size of your app of your update you build your linear system your matrix a in your matrix B exactly with what we have written on the previous slide and then you run your spa solver which should solve H times Delta x equals minus B and this you can use Tedeschi decomposition as we have briefly sketched last time but now you would use sparse variant of classical decomposition but that's something that happens to be very transparent to you if you use the corresponding sparse matrix classes in your mathematical tool box and then you can say my new configuration is my old configuration plus my Delta X so my my update from the previous initial guess and then I'm repeating this process while I'm converged just building the linear system solving it updating the state/territory iterating until I'm done and then I come up with a new configuration X which is now the new configuration of my graph which minimized they the sum of the squared errors of all constraints and this is same the result that he can use in order to build the map of your environment just take the poses as they are what is the best estimate you have and then basically run a mapping procedure based on those pulses and fill the map of the environment that's what's typically done so you correct the pulses first and then use the pulses for your mapping algorithm which is not a slam algorithm anymore and then okay and that point in time I want to show you very trivial examples typically I do that on the whiteboard but give me that I don't have a whiteboard here it is I just want to go through it anyhow because you see a few things you may have not taken to account before so this is a very very trivial example to just consists of two nodes in a one-dimensional world so there's nothing that can go wrong you would think of so you see it we have a node X I and XJ and we assume that there's one constant one observation about them saying X two is a meter away from X I okay so we have our state vector consisting of X our X 1 and X 2 just have two constraints they are we initialize them both with 0 0 and let's say this is our initial guess we assume we don't know anything about them we both put them into 0 ok zette I that 1 2 is exactly our observation in the observation says the node 2 should be 1 meter away from the node I have one so and this is this z12 our actual observation how far to is away from 1 then we have information matrix over here which tells us how certain we are about that and then we can use this to compute our error so our error function is our our actual observation in this case minus our predicted observations or predicted observation is simply the distance between the two notes x2 - x1 in this case initially this is zero so we have our observations 1 - 0 - 0 because the we initialize them in exactly the same way which gives me an arrow of 1 so my error e 1 2 is 1 then I need to compute my Jacobian so I need to derive the corresponding error function with respect to both variables and then of this error function and of course I get a linear term with respect to X 1 and X 2 because if I need to I need to derive this equation here with respect to text 1 next to this will give me the values for X 1 which is positive of course has two negative signs over here which get turn it ought to be positive and will be minus 1 where the derivative with respect to X 2 because this is a negative negative X 2 term do derive with respect to X 2 leaves minus 1 okay then I have my Jacobian and then I can compute my B back door so my B vector is my error vector times my information matrix times my Jacobian so it will be a vector 2 and minus 2 and I can build up my H matrix which is J transpose times Omega times J which will give me a matrix which looks like this and contains of minus ones and once ok now I have my H and I have my B now I can solve my system of linear equations so it means I need to in this simple example I'd say invert the matrix H and then at and multiplied with X and then I should get my update but now it turns out that I have a problem you know to invert the matrix it only works if the matrix has a determinant which is nonzero right so but if I looked at the determinant its determined he can be easily computed 2 by 2 is 4 minus -2 by minus 2 this is also 4 so as 4 minus 4 is 0 so as a result of this this matrix has rank deficiency and actually can't invert it so what went wrong what's the problem that we you know why is it working even for this super trivial example and what turns out to be is that the constraint that we have is a relative constraint between the two notes tells you how far is x2 away from x1 and so it only tells us something about the relative configuration between the two it doesn't actually say us where one is actually in where 2 is in a global reference frame so we basically miss the reference phase or something is called a god freedom that we have and what we need to do is we need to fix one of those nodes so we need to say ok let's take note number one and fix node number one to be 0 for example or to be 1 or to be to whatever and then we can we need to update our system to reflect to reflect that and then we can actually solve that ok we can do this in a very easy way by saying enforcing a constraint that the update on the first node should be zero so there should be no update in the first node basically looks like a Gaussian distribution or constraint of a Gaussian distribution which fixes the first node as the reference frame with a certain uncertainty it's like a prior note that we had and we can add this prior node as a different constant additional constraint to our H matrix which is just a matter isn't one here and it's zero everyone else but basically puts this Delta X to zero so no shift of the first node and if we do this then we can actually invert this matrix because then it's not a ranked efficiency anymore then this element is 3 we have 3 times 2 is 6 minus 4 2 so at the rank it has a determinant of 2 so that looks actually good we can invert this matrix and then actually get an update which says the note 1 should not be updated and the node 2 should be updated by 1 and then we have our configuration 0 & 1 which is the one that actually corresponds to the observation because the observation said we are one step away so this was again a very very simple example in order to do this but the inside that we need to fix add a prior or fix a coordinate saying something that we should not forget so we saw that this matrix is not full rank after we have added all the constraints and even if you had more constraints we will suffer from the same problem always because we basically haven't fixed the global reference frame there's only changes if you have an additional sentence or like in GPS receiver for example which fixes a global reference frame then this problem is automatically be solved and otherwise we need to set one of those nodes as our reference frame for example so we may use the first on node number zero and add a prior as a prior information say ok this should be zero zero zero or this should define my coordinate system and we can do this by adding a constraint to pose number zero which could something like this that post number zero should be the transformation means I have a prior note and this prior node should be fixed at X zero so basically don't move X zero and do everything else with respect to X zero that's one way we can actually do this and if you add this prior information to it then we can actually run our least squares approach get a solution and build a graph based on system and one example how this system is actually running in action you see over here so this was also video I've showed in one of the initial lectures you see a robot moving around let me stop the video for a second what you see here and the zoomed in view is the small robot the mad built so far in the local view and the current laser scan and here you see also the map built so far so black obstacles why does free space and is actually moving around through the environment and just adding one after the other and what you will see in a in a few seconds is that before that here so the robot here comes back to a previously seen place and what you can also see it's maybe how a little bit hard to see in the video but if you would loom in you can see that there's a certain miss metree small offset between the map built when it was Robert was starting and the map built when the robot was actually driving back it's driving back and you will see this map shaking in a second this shaking is a result of this optimization process because we get new observations and the the least-squares abroad shifts the notes so that it minimizes the error introduced by those observations and if I kind of restart and rerun the video you will soon it saw it shaking a little bit will happen now again and the robot re-enters the map you see kind of the map shaking a little bit the more loop closes they have the smaller this effect this because they have Corrections have already been taken out but whenever this happens we actually see small Corrections and the map is realigned and this is a result of the visible result of the least squares approach that is actually running behind that so this is a result of a full slam system which runs ICP registers one scans against the next skin and builds up graph and also looks for loop closures so whenever the robot is kind of close to a map it has been before there's basically an ICP trying running and see do we find actually a good match and if so we add loop closure constraints to this and then you can actually build a simple laser based slam system like that this was kind of the basics of this approach and now I want to go also a little bit into talking about a few things which may are important for you which also are related to for example estimating the uncertainty and so on so we assume that the values of certain variables may be known a priori so there may be some information that we either want to impose the such a system or that we get an additional information where something should be fixed and should not move anymore so how do we actually do that that we can afix a certain number of variables this is similar to the prior that we had we can add one of those prior notes for example saying a you guys should actually be fixed here but the problem is is just a soft constraint it's not really a fix and what can happen is if you have other constraint that they actually pull away this node we cannot ensure that a certain variable has to have a certain value but we want to be able to do this so we want to fix one variable as a result of this this very area should not be optimized so it should actually disappear from our linear system it's not linear system anymore there will be no update on this variable and all the other variable actually should be constrained on that so we kind of want to have as a condition it's like a condition that given this no take this value how about the rest actually look like and we can do this by actually constructing the full system and then simply suppressing the roles in the column that correspond to this variable not to fix this variable and then solve the linear system you can you think about there K now I can actually do this why can we just kind of suppress the row in the column just take it out of this matrix and solve and of course also from the x value and the B vector and then solve all systems and then we actually fix the variables this is a result why we can do this results from the conditioning of Gaussian distributions and if you think about the information space you're in so when we studied the information filter you may have seen that in the conditioning in information space means I can just take out just cut out a part of my information matrix and just ignore the others and this corresponds to conditioning operation of the Gaussian distribution and our H matrix is an information matrix of our overall problem of all the constraints taken together so by taking out cutting away let's say a few rolls in a few columns are basically conditioning the system on these that these variables are fixed and cannot be moved and all others has to be constrained based on the current variable that those system has and therefore I can actually do this I just can cut away cut out the corresponding roles and columns from my H matrix solve it and this means I'm actually fixing those variables that I've cut it out and this is something that you can actually use if you want to constrain something the next thing I want to look into is actually be uncertainty related to this so I told you right now that this H matrix is an information matrix for my overall system so it provides me with information about the uncertainty of the overall estimate up to the linearization point so given the linearization point given via converged and we are under the approximation of this linearized function this is an information matrix so in order to obtain the uncertainty my covariance matrix actually to invert H and invert this matrix and then I have a covariance matrix and this covariance matrix tells me something about the uncertainty of those variables the only problem that we may need to take into account that if we are inverting our matrix H we typically get a dense covariance matrix so an inverting this matrix so we get a dense covariance matrix is a computationally costly operation and as a result of this we may want to also do some tricks you only compute part of this uncertainty if we need it but if you need the uncertainty of the overall system then that's typically the way to go again there are some approximations you can do because you're typically only interested in the main diagonal blocks if you want to estimate the uncertainty of all the other variables with respect to one and this this is there are special ways to do this which are computationally more effective what you also need to take into account is relative uncertainty so quite often specially if you look for loop closures you want to actually say what is actually my relative uncertainty of my current post with respect to a certain previous pose so not you know necessary interested in a global reference frame but what is my pose uncertainty relative to another post in order determine is this a possible loop closure yes or no and because how can we do this how can we compute the relative uncertainty between X I and XJ and what we can we can solve this is that we actually compute the full matrix H and then let's say want to compute the uncertainty of X J given X I that means we can fix X I and make X I kind of our reference frame and then say given that X is reference frame what's the uncertainty of this X J with respect to this reference frame so we fix X I so we don't optimize at the more we fix the by suppressing the rows and the columns corresponding to X I and then we need to compute theory the inverse of the whole matrix and then take out the JJ block from that matrix but again their techniques we couldn't do this more efficiently if you're only interested in a subpart of this inverse you can there ways for computing only this block JJ of your of these of the inverse of the matrix and then this block this inverse the the block JJ of the inverse actually gives you the covariance matrix of X J with respect to X I being fixed and this tells you of radical said this is for example being used when you build Maps so this was one of the example the robbers currently here and computes the relative uncertainty of all the nodes with respect to its current pose in order to check which other nodes could be close to me so I may inspect them for loop closures and you see kind of the robot sits over here and there's just a very small number of nodes which it actually needs to inspect because for all the others the uncertainty is too small because it has already built a high quality map so that it knows it doesn't need to look for loop closures to other places and that's one way where this uncertainty information is important if you want to decide at which other positions you want to check for loop closures and this actually brings me to the end of today so what we have introduced here is kind of the back end part of the slam problem using least squares approach of this graph based slam or even a special variant this post graph based slam problem a postcard is again a graph which only contains the poses of the robot and doesn't take into account landmark features as a general slam graph should actually do and we have discussed how we do this how we can use the ideas of least squares in order to formulate that problem how we can actually build up this matrix H and the vector B that are needed for solving the system and the key inside in here what that H is a sparse matrix and if H is sparse we can actually solve that in an efficient manner and so this is one of the key practical important insights not to solve this even for large systems having thousands ten thousands or millions of variables otherwise that would be computationally not feasible at least not for building online systems in this idea of pulsed routes are something that you find today in a large number of different slam systems also partially in localization systems today where the informations are used to build up this graph this post graph online and then the graph is optimized sometimes only parts of the graph only the recent history some in an approximated fashion some using hierarchical setups to save computational resources in order to tackle online computations some approaches use smart outlier rejection techniques to deal with unknown data cessation so there's a large number of variants that you may need to take into account but the overall core part of this is actually very similar of those algorithms to what I have presented here and they're also libraries out there and such as g2o gtcm or saris are the most popular three months which can help you to perform those optimizations in an efficient way but here in the course in the practicals we actually trying to build that system completely on our own so that you're actually able to get hands-on experience how to build such a system and do the homework assignments and you can actually play around with your own two DS slam system and build for example a postcard baseline system so that you can put hands on to every part of the problem and are able to derive all the jacobins from by hand and at least for the 2d case do this and in a very easy fashion for the inbuilt your own slam system and understand it from the core that's one of the things that will happen in the upcoming homework assignments so thank you very much for attention and I hope you got an idea on how graph based slam or post graph based slam works and we will extend that in the future to more general post graphs robust optimizations and other things thank you very much

Channel: Cyrill Stachniss

Views: 19,485

Rating: 4.9907408 out of 5

Keywords: robotics, photogrammetry

Id: uHbRKvD8TWg

Length: 71min 55sec (4315 seconds)

Published: Fri Apr 03 2020