ICP & Point Cloud Registration - Part 3: Non-linear Least Squares (Cyrill Stachniss, 2021)

welcome to this third part of the lecture on point cloud registration and looking into the icp algorithm and today we want to look into the most general let's say non-linear least squares approach to point cloud registration so so far we have discussed in finding point cloud registrations given non-data sensations where we looked into finding the optimal transformation to transform the two-point clouds on top of each other and then in the second part of the lecture we looked into the question what happens if we don't know the data association and go for the icp-based approach where we find the closest point in another point cloud in order to estimate correspondences in the third lecture here today we want to look into a least squares approach where we don't use this svd-based registration but look in a somewhat more general formulation of that which in the end leads to a gauss newton-based iterative alignment and see what we can do with this for point cloud registration so what we have seen in the previous lecture we had this example here of those two bunnies and the goal is to align those bunnies so for every point in point cloud number one we look for the closest corresponding partner in point cloud number two based on a nearest neighbor criterion so we look for the closest point and then say this is a correspondency and then try to minimize the distances between those two point clouds and actually minimizing a squared distance and if you run this video we see different iterations which are ongoing here which try to find the data association then perform the alignment step then again look for data station do the alignment step and so on and so forth until after a couple of iterations those two point clouds are aligned with each other so and we started out originally with a very simple form of misalignment so consider we have two point sets y and x over here in some different coordinate systems one could be a global coordinate system which is available some form of map form this could be your local sensor frame it could also be two different sensor frames that doesn't really matter and then we have the correspondences over here indicated as a set c which are these gray lines knowing this point and these points they should correspond and those two points should correspond and initially we assumed this to be known then in the second part of the lecture we relaxed this assumption and tried to estimate this c and then based on this information we can estimate the parameters of rigid body transformations or rotation matrix r and the translation vector t to perform the point set x n into a point set x and with a bar on top of here which then should be in the coordinate system of the point set y and this should happen under the constraint that the sum of differences between the point cloud the red one and the transformed one should be minimized and then we could apply that transformation and transform the points the the blue points onto the red ones and assuming that there's some noise in this process there may be small variations in there but overall we should get the alignment that minimizes this squared error so we are minimizing this error function over here which is the distance between the point sets in the target sets or the red ones minus those transformed blue ones and then trying to find the best transformation so that this error is actually minimized and that's the least squares approach and we used svd-based technique in order to find this transformation and in the lecture today we will not use this svd-based formulation but use general least squares approach and this has the advantage that we can take different error functions into account and not just these simple ones we also have different ways for dealing with uncertainties of that of these points and different advantages why in some situations we may want to go for the least squares approach and leave the svd-based alignment if svd-based alignment works well for you it's compatible with your error function you can do everything you want to do there's no reason to go to these least squares to this least squares approach you can do the same with your svd-based technique if you however want to add additional information into your arrow functions maybe in the end want to go to a non-rigid alignment of point clouds if you know that this object can be deformed then you also need to go towards a least square solution like the one we are shown over here okay so the goal again is to find the parameters of a transformation that best aligns my data points and we looked into different parts of the lecture in part number one we looked into known data association in part number two we said we want to estimate the data association leading to the icp approach and then in the lecture today we want to look into the least squares or robust least squares approach so in the first part of the lecture known data association and here we could derive an efficient computable optimal and direct solution for finding the rigid body transformation so everything perfect in this setup nothing to do here so if you know your data association part one is sufficient you don't need to know more and typically we however don't know the data association so then part two becomes interesting for you and then there is no direct or optimal solution available and we need to basically estimate or guess correspondences and then end it up with an iterative approach in order to align those point clouds we can also illustrate this the icp approach so we typically have our point set we we look for points either sampling them from a mesh or selecting points based on a layer range scanner for example which generates three data points and then we look for corresponding partners in the other point set typically the closest point with the smallest euclidean distance for example and then we use this information in order to minimize the distances so transform one mesh onto the other one point cloud onto the other and then we are iterating this process of um finding new correspondences given this improved initial guess and iterate this procedure until convergence and we also looked into different variants over here should we take into account all points for this alignment or just a subset and how do we find the data association or a good data association so we looked into point-to-point metric also point-to-plane and other techniques in order to try to find compatible point sets for the alignment we can weigh our correspondences and also look a little bit into outlier rejection what happens if a certain number of points are caused by outliers for example when operating in dynamic environments and we generated solution for that which are kind of variants of the icp algorithm so for finding those correspondences there are different ways on how to do this and again finding a good data association is key and we had some kind of um let's say best practices so investing in your data association is key if your data station is wrong you're not getting the correct alignment so putting a lot of brain power and finding good data sensations is key and this still holds for this part of the lecture nothing changes in here if you have an initial guess for example from wheel odometry or from other sensing modalities you should exploit that information because a good initial guess helps you to do better data associations and in this way end up within better alignment we also have seen that normal based metrics typically work better so um if you for example use the point-to-plane metric it typically works better than the point-to-point metric we'll see something similar here today and we need to take care of outliers especially in dynamic environments and today we want to look into the point load registration using a non-linear least-squares approach so basically going into the standard least square error minimization and not exploiting kind of the special properties that we did for the svd-based alignment in the first part of that lecture but going to a general approach and again you may ask yourself why should i care why do i need a non-linear least squares approach to do this and there may be multiple reasons so the svd solution basically assumes point-to-point correspondences and maybe you have different ways for defining your correspondences you have more complex error functions so to say which then require a more general formulation because you can't put that into the svd-based alignment also the least squares approach at least if you have gaussian uncertainties in your measurement noise is a statistically optimal solution um so you can better better take into account uncertainties if you know that points have certain let's say more complex shapes than just a single weight for your correspondency but you have more complex shapes of your alignment you may take that into account in the least squares approach those information is often hard to get but if you have it you could exploit it better in a setup here and what you do then is you run typically a gauss newton or living back markward approach typically um here we use gauss newton and perform an iterative error minimization in order to come up with a solution and we want to do that as a starting point using the 2d point-to-point icp so something we can perfectly solve with the svd-based approach but as an illustration show you how we can do this and then in the second step generalize this to the point-to-plane metric for example okay so let's start with the least-squares approach for 2d point-to-point registration so we have point sets in 2d not in 3d this makes a lot of the computations easier and then we want to find the rigid body transformation consisting of a 2d shift vector our translation vector and 1d rotation matrix in order to align those point sets so in the 2d plane okay so very similar to before we want to minimize the euclidean distance between the points after the alignment we have an error vector which tells us the discrepancy for one point um correspondency so where what's the distance between the transform point and the um and the actual point where it should be uh we can put our transformation parameters in here of our body transformation rotation matrix r and translation vector t here in this example this is now a 2d vector and this is a two by two matrix or rotation matrix so that the parameters for the transformation are called t x t y and theta which is the translation x translation y and the rotation um in order to find the alignment so explicitly this is the arrow vector we are going to minimize and the question is how do we perform this error minimization and that's kind of the standard approach you compute you take your error vector if it's non-linear and that's the case here because it's a rotation matrix so in this matrix r there are sine and cosine functions in there therefore this is a non-linear error function we need to linearize it so compute a linear approximation of the original problem and then work with this linearized function we basically compute the first derivatives set it to 0 solve the remaining system of linear equations which then provides us with a solution to the to the problem iterating this of course and so in order to do the next step in order to linearize our arrow function our non-linear error function we need to compute the jacobian of this function so we need to take this error function here into account and compute the jacobian that means we're taking this error function e i need to derive it with respect to the three parameters so we take this error function e we derive it with respect to t x we derive it with respect to t y and we derive it with respect to theta and that's what we're going to do which leads us the jacobian in this case this is a two by three matrix exactly we have a two dimensional error function because we have the um the function has an x and y component and it has three parameters so we have two by three jacobian again the jacobian to be written in this form consists of the partial derivatives of my error function with respect to the individual parameters so here we have kind of the x dimension of the arrow function the y [Music] dimension of the error function with the two-dimensional error function and derived with respect to the three parameters so which gives us these six elements that we need to compute in order to formulate the jacobian and this jacobian is then the common component that i need in order to generate a linearized error function and then i'm just following the standard least squares procedure i'm just using this jacobian e in order to formulate the system of normal equations and then solve this system and come up with a next a better estimate of my parameters t x t y and theta okay so how does it look like we need to compute those partial derivatives that's the next thing that we need to do so we take our error function into account and need to now derive this error function with respect to t x with respect to t y with respect to theta okay so we look into this error function here we need to derive it for example with respect to t x so t x the translation vector x only pops up here in the first dimension of this translation vector this part is independent from t x this is independent from t x this is a linear term so if i derive t x with respect to t x i get out of 1. this is exactly what we're getting here a 1 over here if we take the first dimension of this function it derives with respect to t y t y never pops up because this is the second dimension this is the third dimension it's not in here it's not in here it's not in there so this gives me actually a value of zero and the same holds um for the second dimension of this function if i derive it with respect to t x i get a zero in here and t y one so these components the first four are just zeros and ones so everything is linear in here there is no non-linearity you can directly see this because we just have this two-dimensional vector here which has t x and t y as linear elements in there there's no whatever quadratic function or other non-linear form the only non-linearity that we have is actually this rotation matrix over here so this is a rotation matrix that means it contains sine and cosine functions depending on the parameter theta over here and we need to derive it with respect to theta so those in those elements the theta doesn't pop up so this is the only thing which remains so what we basically need to do we need to compute the first derivative of that rotation matrix with respect to theta because this value here is independent from theta this is then just a constant factor so we can write it in short form the first the two by two matrix here is the identity and then we have the um the first derivative of the rotation matrix with respect to theta multiplied with the vector x n which then gives these missing two components why do these components look like that um you can see this very easily if you compute the first derivative of your rotation matrix so you take your rotation matrix this is a standard 2d rotation matrix describing rotation around theta and you compute the first derivative with respect to zero that means we compute the partial derivatives of those four entries so the cosine the first derivative of cosine theta with respect to theta gives us minus sine of theta that means this cosine theta turns into minus sine of zeta and the sine turns into the cosine you can see over here and then this is the first derivative of our rotation matrix with respect to theta so i'm just taking this matrix and multiplying it with x n a two by two vector which is the point coordinates of my original point set and then this gives me these uh this vector over here consisting of these elements so this x n written in this form and y and are the x and y coordinates of this vector x n so these are the original coordinates of my points and so this is my jacobian this is the elements that i need to compute and with this jacobian and the individual error vectors over here i can now formulate my least squares problem so what do i need to do for this i need to compute my normal equation matrix which takes into account the jacobian in order to set it up i need to compute the right hand side of my coordinate system taking into account the jacobian as well as the error vector evaluated in a position and then i need to solve my resulting linear system so these are the steps that i need to do and then i can just follow the standard least squares procedure so this is my jacobian as a result of the jacobian i can compute my matrix h n which is the jacobian transpose times the jacobian holding only for the point corresponds n i can do the same for the right hand side b n so the vector of the right hand side is j transposed times e the error vector for n and then i need to sum over all correspondences so a sum my matrix h the sum over this matrix matrices h n and same for the right hand side so my matrix h this form vector b has this form and then i have all the components that i have and i need to solve um h times delta x so the update in the parameters so this vector x is now here the parameters of my transformation so t x t y t theta uh d x d y theta and basically the increments of that and so i need to solve this system which i can in this form very easily solve by just inverting this matrix this is a three by three matrix remember h is a three by three matrix and i can easily invert a three by three matrix so this is an easy way just inverting the matrix and getting my my update of my parameters so i update my transformation parameters and i iterate until convergence so the standard least squares approach so what i've replaced in here is basically the svd-based alignment that i used in the first part of the lecture we derived in the first part of the lecture by the standard least-squares approach and the advantage is that i can take more complex error functions here into account which then will lead to different jacobians of course so i need to always compute my jacobian by hand and depending on the internal representation that i'm using my jacobian will change as well but it allows me to do a more general alignment so let's see how that looks like so this is my the example that we already used in the previous part of the lecture uh the two point that that should be aligned the blue one and the um orange one we are looking for closest points so we're going over all those points in the blue set trying to find a correspondency in my kind of reference scan to which i want to align it to so this would be the y and this would be the x n over here and then i'm estimating my correspondences just based on the nearest neighbor assumption over here and i compute the alignment after the first step of the least squares approach i'm actually ending up being here in this configuration and then i'm basically repeating this process again i'm re-estimating my correspondences that how the new correspondences look like i compute my alignment and in every step you can see some of the um the correspondences will change and we very slowly go a step forward until the two point clouds are finally aligned so in this case over here you can see both point clouds overlap perfectly so we have found the transformations which align the original point set with our target point set and we can actually plot the error over time so these are the iterations this is the arrow and we can see how the error basically goes down and something like after whatever in this example whatever 15 16 iterations something like this we actually little situation where nothing changes anymore and the system has converged this was a simple 2d example something that we could have solved as well with the svd-based approach there was no need for going to the least squares approach in this example over here but just as an exercise we started with this in the next part of the lecture we want to do the same thing with the point to plane um distance or point plane metric so um looking into so leaving these point-to-point data station but going into point-to-plane just as a reminder point-to-point point-to-point metric was taking the different point sets and basically finding the closest points and trying to minimize these distances what the point of plane metric does it basically takes those distances and projects them on the normal vector of the individual um the normals that are generated from those points or from those surfaces and then only computes the projected error so we also had this other plot which looked like this so for every point in my point cloud here i'm looking for the course of the corresponding partners and then can estimate the normal and basically project the arrow onto the normal and then not taking this distance but taking this distance into account and this is basically the same as using the normal vector that i'm computing and multiplying it with the error vector that i had for my point-to-point alignment so with this dot product i'm basically projecting my error vector e onto the normal vector and then compute the sum of square distances over this function okay so what we need to do we need to compute normals and we can compute normals in a point cloud typically by just iterating over the neighbors so for every point we look to its neighbors and that and then compute here a normal vector and that's basically what you see here simple estimation of the normals you can do also more advanced techniques you can um compute basically take the points in local neighborhood into account and look for the eigenvectors um and eigenvalues of that are generated by those points and then take the eigenvector that corresponds to the smallest eigenvalue and so this is basically the normal of the surface and then you can actually get this normal information or you depending on the noise that you have you just take the cross product so there are different ways how you can do this in an either very efficient way or in a slower and more accurate and more robust way whatever way you do it you just need one way for computing this normal information okay so then we said our objective function should be the sum over the error vectors multiplied with my normal vector so if i expand it this will now be my error function or my objective function that i aim at minimizing so what do i need to do i need to compute again the jacobian of this error term so this is now my error function which sits in here my arrow vector this is the squared error and this inside here is zero vector and i need to linearize it and compute the first derivative with respect to the parameters which are again t x t y and my rotation component theta so i have my jacobian my arrow vector over here but derived with respect to t x t y t theta the thing to note in here is that this expression over here is a 1d error function so this is a two dimensional vector this is a two dimensional vector but we're computing the dot product of this two two-dimensional vector so this turns just into one single number as a result of this this jacobian is not a two by three matrix and it was before it's now one by three one by three vector that we have in here right so we just have three columns that my jacobian has here and now what we need to do is we need to compute again as before the first derivative of this whole expression with respect to t x t y t theta and what basically changes here we had the expression we had before but now i have the dot product with with and the normal vector so we have basically need to multiply the terms that we had before with the x y and x and y component of the normal vector over here so basically the first two elements here again this is a linear expression this so this just gave basically a constant of one so as a result of this if we compute then the dot product we have here the the components of the normal vector coming to the game and for the third component we had this longer expression from the rotation matrix and again we have here parts of the normal vector that come into the game in order to compute here the jacobian and then we simply take that jacobian and then proceed as before so we are computing our h matrix we are computing the right hand side iterate as before so we start again with our example we had our we have our point-to-point correspondences here we are always showing the basically um correspondency to the closest point which then was used to compute where the normal was used in order to compute in the error function and we again can minimize this error function and compute the alignment which now takes the normal information for this alignment into account and what we can see here um so you have to be careful with this plot because the scale is a little bit different as compared to the plot we have seen before but what we generally see the arrow drops faster to a low value so in this example here after kind of 10 or let's say 11 iterations we converge before i think it's 16. um so but again the scale is a little bit different of the axis so you can't compare those plots immediately you have to take care if you compare those plots and so this is an example of the 2d point-to-plane metric now using the least-squares approach for minimizing it um the problem of the point-to-power one is a problem issue of the point of plane metric is that it's not symmetric so depending if you compute the error from seeing from one part or the other point gives you a different result because the normal information is different in one surface or point set compared to the other so if we want to fix this and propose a symmetric point-to-point metric which takes the which is symmetric independently if i'm using a normal vector from one cloud or the other cloud that's what we may want to have so just as a comparison the point of plane a point to point was just computing the difference between the points the point to plane was taking basically one point and computing the normal information using this normal information and taking this error vector the same one over here and projecting it on this normal vector so this was basically the result that i was getting but obviously this would only be symmetric if i would have the same normal vector here for this point it would have the same normal vector than this one which typically is not the case so if you for example have these points with different normal vectors so this is the same normal vector the one we used before and now this is a different normal vector over here then the question is how can i define a metric which is symmetric so it can take both normal vectors into account and a fairly simple approach which has been proposed a few years not many years back actually quite recently and very easy solution is just average those normal vectors just some or just sum them up so what we can do is we can compute the plane as the um average of the two normal so we compute the new vector n over here normal vector which is simply the sum of these two normal vectors that i had in here and then i'm using exactly the same error function as i had before but now multiplying it not with the normal vector in um of this of this blue point over here but basically this new normal vector which is the sum of the the other elements um so basically this varies an approach very similar to the point-to-plane metric i'm just taking one additional vector into account so what i'm coming up with is a very similar formulation except that i'm replacing the single normal vector with the sum of two normal vectors so the only additional work that i need to do is to compute the normal vectors also in the other point cloud before i had to do it only for one point cloud and now i need to also compute the the normals for my second point cloud but this is the only change that i need to do and if i use this error function compared to the other error function i'm actively getting a better result when aligning point sets so this is an example kind of the what you see here is the result that you obtain with the symmetric matrix this is point centered point to plane this is point-to-point metric you basically have the error before the icp iteration and then the error after the icp iteration so depending on the initial error you had you're basically mapping it to the output error and so what you can actually see this plot is always lower than the point to plane metric which is always lower than the point-to-point metric so you can see you get actually better results for the alignments using this symmetric metric over here so fairly easy let's say improvement that you can do which is very easily implemented the only thing that you need to do is to add a second vector over here and this simple change leads to a better conversion speed because you can quick quicker reduce the error then potentially make better data associations and then faster converge to the right solution and also you can show that increases the basin of conversion so the more error you have in your initial alignment still you get good um data associations and as a result of this um come up with a with a better solution so aligning your point clouds correctly despite a larger initial error so this symmetric metric over here is probably thing you want to start with when you build your own um icp solution the next thing that we can take into account in the context of the least squares approach is to do an outlier rejection so in terms of least squares estimation we often refer to this as robustly squares where we don't take the residuals into account as they are but we can downway residuals which are likely to belong to an outlier and there's a this is a standard approach which is available in the long time on um reducing the impact of data cessation errors or outliers in our estimation techniques and there's something which is often referred to as robust kernels or m estimators so what we are doing if we are computing or doing a standard least squares is we are basically have a quadratic error function right and so the further we go away in the x axis the larger the error gets quadratically that is the relation to the gaussian distribution where we are basically computing the mean of the gaussian distribution with this form and but the problem with this is that the probability of points with very large residuals is actually very small if i assume this gaussian and they basically drop to something close to zero until whatever three four five six sigma away it's basically zero afterwards and this is not what happens in reality there's a certain probability that we have strong outliers typically because something happens in the scene that we haven't probably take into account or model when designing our our noise values or error functions so what we can do in this robust estimation is to use a different kernel function or do using a different function instead of a quadratic function to it we squeeze our arrow vector this can be just linear so just the absolute value um this could be things like um here in the um around the actual error we keep our quadratic form but then at some point in time we turn it into a linear function though that the growth is only linearly um outside this area over here which basically says close to the correct solution we are assuming our gaussian noise but further away we are kind of down weighing the impact of those points not taking them quadratically into account but linear so this is for example a huber a functional huber kernel that is used and you need to basically define your kernel function depending on your on your outlier distribution and so for example these values give a smaller weight to points which are very far away you can hear the impact even goes more or less constant so this will basically not penalize um an error larger if it's two or ten basically doesn't make a big difference anymore in this kernel so depending on the error function which tells you something about the assumption that you make about your outlier distribution and there is again a large set of different kernels that you can actually use there's a large family of different kernel functions and you basically need to find the kernel function which best fits to your data and how is this taken into account so you basically squeeze your error vector through one of those um robust kernels and so as a result of this your jacobian will change and you can actually turn this into a weight factor that you put into for every error term so basically every error term gets an additional weight and basically downways those errors in the minimization but again the choice of this kernel function depends on the outliers that you're actually experiencing here is an example of different error functions um so this is a quadratic error function over here and this is basically different forms of down weighing um the impact of outliers so this is basically the error term that you get the further you're away from your in this case from the zero point which is the minimum error configuration um and this the shape of those kernels basically turn into weights um so the weight of one which is shown over here corresponds to the quadratic term and depending on the different kernel shape that i'm using i'm basically reducing the weight the further i'm away from the zero error configuration and what you can do is you can take into account these different kernel functions by not just selecting one of those kernel functions but there's actually a family of distribution where you have this parameter alpha over here which basically interpolates between those functions over here so you can start um with alpha equals to two you will get this quadratic form and as you increase this value you are basically going moving down here um and reducing the impact of outliers and what you then can do is you can actually optimize your software list squares problem at the same point in time optimizing your alpha parameter so that you're basically adapting your robust kernels to the actual outlier situation that you have and so this was an example i think i've also showed in the second part of the lecture this is an example on the kitty highway scene where you have a car driving on the highway very little structure around and then a second car comes and overtakes the actual car and the overtaking car provides a lot of structure so the icp algorithm often hooks to this car and aligns the car against the other moving car and this of course screws up the point cloud alignment serial in the motion estimation of the car quite substantially and this happens if you use for example a huber loss but if you take this this adaptive kernel into account which estimates the shape of the outlier distribution for the current situation then you can actually perform better and this is an example a video where you can see the car driving around and building a map of the environment and for the different colors here are just kind of the different type of objects that we are seeing so some semantic information and we first can run this with a fixed kernel and you can see a second car showing up here very soon in blue over here and it's approaching the car and soon it overtakes the actual car this car will register its position against the moving car over here and so it leads to failure in the registration because the car basically hooks to the other car and localized with respect to the other car if you use this adaptive kernel approach you see the car approaching over here and then this alpha parameter will drop to very small values which basically performs a very strong outlier rejection and then this car doesn't hook onto the other moving car and you're able to better deal with the outliers and not lead to a wrong alignment just because your outliers are very prominent so in practice what happens is you need to take into account your outliers if you want to build up build up robust registration techniques and you need to do this properly in order to adapt to the situation so there are two ways you can do this on the one inside you can use some heuristics or something that you know about your problem in order to reject outliers if you know that only certain parts of your environment is potentially moving in other parts of static you can actually exploit this information because you put background knowledge into your process this can be seen as good because you're exploiting additional knowledge you will get better results but it can also be seen as bad because you're exploiting something which is problem specific so your solution will not generalize well into other situations always kind of the trade-off that you need to do and there are different ways um of using those heuristics um that you can exploit but and the other thing is to use this more general methods like this kernel rejection techniques so that you can exploit the best of both worlds so if you have some information about the environment use it explicitly to identify what is a potential outlier at least not taking it into account in the initial registration steps and then combine this with a robust kernel in the easiest case it could be whatever uber kernel but these adaptive kernels typically perform better because they can adjust themselves to the underlying outlier distribution which is better for you and you can also ask yourself if you build up one of those systems are there certain parts of the environment which i know they are static so i can rely on them more than on others you can ask yourselves do i know something about my ego motion am i'm driving on a car the car is not going to move sidewards for example at least not substantially and these are all information that you can take into account in order to perform better and this is something i also talked about in part two of the lecture what you can also do is if you build multiple of those techniques let's say you take the point-to-point algorithm you take the point to plane you take the symmetric one maybe you have a strategy where you say okay let's take additional knowledge into account and estimate the pose but maybe i also want to have a more general approach which also performs well in other situations so you can actually build multiple registration techniques or multiple what we sometimes call sensor odometry estimation techniques use sensor information or to estimate a dormitory what you then can do is you can actually combine multiple of them so let's say this way you have this vehicle moving through the environment you have different odometry proposals let's say one taking robust kernels into account one using point to point one using point to plane one using additional color information one using semantics um whatever you name it you can generate different proposals and so what you then can do is you can check how well is the result that this proposal estimates actually um in line with the motion and you can take basically join up multiple estimation techniques and then do some let's say sanity checks something you know about your vehicle for example to exploit to see which of those proposals are likely to be wrong and which are more likely to be correct and then select the a proper registration or odometry in every step in order to come up with a robust motion estimate and it turns out that there's actually there's no best solution so different algorithms which are chosen different times in the algorithm you can see here this is a generalized icp which is basically a combination of this point-to-point and point-to-plane registration technique and point-to-point are the most commonly picked ones sometimes you want to use color information so this is basically how often has one of those algorithms been picked in typical driving situations where it performed the best and you can see there is no best solution there are always situations where a certain algorithm performs better than others and combining those different approaches and then deciding which is potentially the best alignment i can get in the current situation can be exploited if you really three for building robust estimation algorithms so a few remarks from practice you should always exploit your initial guess if you have an initial guess so this is one of the recommendations never ignore an initial guess that you have this initial guess will be valuable for you because it probably gives you a better database in the first place and then your registration algorithms will perform better this could also be things like a constant velocity model so if you don't have let's say an additional dormitory source you may take into account the just repeating the movement that you have estimated before if you do an incremental estimation technique because very likely you're moving with a vehicle that is driving at a constant speed it's not changing speed all the time and this can be taken into account in order to come up with a better initial guess just a constant velocity model for example as a general remark these the normal based distance metrics typically perform better than the point-to-point alignment there can be other situations but from my experience is that um the the those which use normals typically perform better so for example this symmetric approach is one of the things i would be would be my first starting point if i build up as gen registration technique because it's short to be better than point-to-point and point-to-plane was a standard for a long time so that's probably a good start is to start with this symmetric approach as i said before whenever you have information about your outliers or also which part of the scene are likely to be not outliers exploit this explicitly so this is what i call informed outlier rejection applied if it's possible it will lead to better results if you can exploit specific properties of your scene of your problem but of course you need to take into account that this may limit the applicability of your algorithm if you want to do a very general approach which works in all environments and the adaptive kernels um we found them quite powerful in order to adapt the current situation to the uh or adapt the robust kernel to the current outlier distribution and then perform good results and not needing to commit on a specific kernel beforehand so these are kind of some remarks that you could take into account when you build your own registration algorithms um there are a few more so um if you have this what you call sensor odometry so where you estimate the movement of a vehicle based on an icp or icp-like algorithm over time you can do actually a couple of things so first if you have multiple sensors take multiple sensors into account even if you for example have a laser scanner and a camera and you want to estimate using both sensing modalities you can treat them independently and estimate transformation parameters just based on your camera even if it's just a single monocular camera you can at least derive five degrees of freedom out of the six degrees of freedom from your camera and typically cameras provide you with a really good orientation estimate so you can take actually multiple sensors into account either fusing them using them jointly or using one as a kind of sanity check for the other and only if all of them agree you say okay that's a good way to go it's one examples velocity constraints or other dynamic constraints your car won't accelerate from zero kilometers per hour to 100 kilometers per hour within a second or very unlikely it's going to do that so exploit constraints because they allow you often to identify certain failure cases um and what the where the incremental registration does it provides you incremental corrections so you're always registering against the previous skins or the previous five skins or ten scans but typically not much more so you're building basically a local map and always a line against your local map if you really want to get drift free in the long run you need in some point in time go to a slime algorithm and then probably perform some global optimization some global these squares on aligning your trajectories also changing a trajectory in the past into in order to close loops correctly and get globally a good estimate so the incremental odometry only keeps you active for a while the further you go um the more you may then want to turn it into a slime system his last remark especially when slam comes into the game from my experience the uncertainties that you're estimating with an icp-based approach are typically not that great so typically you tend to overestimate the accuracy of your alignment or the precision of your alignment with respect to what happens in reality so i tend to not trust the uncertainties you get out of your skin alignment too well again there are special techniques also recently been proposed that try to do better on that but in general i would be a bit more bit careful with the uncertainties that you get from the skin matching estimation technique um but you can take them into account to some degree but put it in with a use it with a grain of salt and you can integrate it into slam algorithms so this is an example of a lidar by ladder based slam system that we have been built or against bili or the main contributor here which uses icp with the protective data station basically matching the current laser scans against the circle-based map representation again taking the normal information into account using robust kernels so that you can then build maps of the environment so these are all sequences of the um kitty data set and building maps of the environment using this sensor data correct for loop closures so that you get a proper trajectory estimate out and map of the environment so basically estimating where the sensor was and building a map of the environment at the same time and that's what you can do is if you combine the hotdog registration techniques that we have discussed here with slam or global optimization techniques and with this i'm actually coming to the end of what we call the rigid registration so where we have one rigid body transformation to transform one point cloud into another one we can however go a step further so there are different applications where autonomous systems or intelligent systems are faced with deformable objects like me as a human i can deform i can move my arms and these are things that a mobile system or robot may need to take into account so they are steps towards non-rigid registration they actually have been around since quite a while so already 20 years ago people investigated into non-rigid registration of point clouds for example seeing which part have changed may be able to deform some aspects over here a lot of the work has been done here based on humans so humans have a certain skeleton a certain structure and we can can we exploit the skeleton structure in order to perform a matching and knowing that my arms can only move in a certain way and this of course information that we can exploit and we have been using the non-registration also for other tasks like dealing with growing plants if you have 3d models of plants taking at different points in time you may need to align or want to align those points or to track the growth of the plan for example over time and you can do this by transforming one point cloud into another point cloud in in a in a non-rigid way and this is something where you need those least squares approaches as well you typically have more complex cost functions into account and sometimes even the least squares methods may not be the best choice there are other approaches um taking deformations differently into account and then using finite element methods for example that's something we're not going to cover here but the with the least squares approaches you can actually go pretty far in here and what typically happens if you will have additional cost terms in your optimization problem for example taking into account that you don't want to change the topology of your theme at least not substantially only to allow for small changes and different things like this so this is an example how that could look like so this is a point cloud of a plant at day one day six and day ten so this is a tomato plant and um you see the point clouds and you can imagine that may be very tricky to align this point cloud with this point cloud to for example estimate how has the stem been growing what's the change in the size of the leaves things like this and but what you can do is you can actually try to do this and again a tricky way is how does the data cessation look like and you especially for these type of non-rigid registration techniques you need to invest a lot of brain power into the data cessation between those points and so this is for example two point clouds which i've been just a day away and you can see here is a small leaf is branching out here so this here the plant only has two leaves here there's three leaves and if you want to do the registration it's helpful to exploit additional information like in this case sorry skeleton information that you have over here so you can fit a skeleton into the stem and into the leaves and try to make the data station based on the skeleton just exploiting how plants typically grow and also saying yes you can there can be new branches coming out at some points so that you then need to do the data station only based on those points on that skeleton um and again you need to take into account that elements have been growing and you may want to have different error functions being taken into account for example taking into account that transformations shouldn't change along the path so what the approach is doing basically every of those dots gets an own rigid body transformation because everything can be moved changed a little bit or not even a rigid body transformation it's even a fine transformation because the thing can also grow and but you what you don't want to do is that the fine transformations of those points change dramatically so you're going to want to perform some regularization over the change in the fine transformation along that way and then you want to have certain regularization terms taken into account so you come up with a cost function which is more complex but can realize this non-rigid registration that you can match one skeleton with another skeleton even if it has grown new leaves have appeared so that you can then actually take your skeleton match it against the other one and then actually come to an alignment of course the leave the the point thought is still slightly different because it has been grown the leaf has been grown but you can actually perform the alignment well for this skeleton and then use the skeleton and transfer the information back to the overall point cloud so that you then can even realize more complex alignments and then determine which parameters for the plant has changed you can use this for phenotyping for deriving phenotypic traits from this information and then also actually if you do this with in between all the time steps track the development of the plant over time so this is a small example of a video so these are the points where you have actually taken the scan and the uh and the red one is always an interpolation between those two clouds so whenever it overlays with this one you see okay this is the exact measurement the point cloud and then you can actually turn to interpolations and estimate how the plant will actually grow over time and perform some visualization interpolating the plant grows and then actually track phenotypic traits between those plans over time although you hadn't had measurements at these positions another thing that you can do with non-rigid registration again often people are using this in order to deal with humans so a lot of the literature looks into humans and aligning close which is non-rigidly moving and taking this information into account and so there's a whole set of more complex registration techniques than just the rigid body transformations that we have discussed here i just want to show this as a outlook where you can go if you want to dive deeper and are interested in those techniques there is a lot of literature out there for aligning humans or other non-rooted objects with this i'm coming to the end and i want to point out a few resources that i found useful if you want to dive deeper so the first thing is the jupiter notebook by igor bogoslovsky which provides you a very easy dive into an examples everything done in python performing the simplest form of point-to-point registration using the svd approach then using the least squares approach all done in 2d so some of the examples that you have seen here actually stem from his notebook and then also performing outlier rejection with a simple robust kernel these are examples that you can see here with a few lines of code in python you can actually generate that of course you can do better you can take more heuristics into account but i found this a very good starting point both for learning but also if you want to use this as a starting point for further projects so highly recommended uh to look it up then there are also libraries out there like open3d one example provides you with existing point to point or point to plane registration techniques if you just want to use it as an out of the box tool you don't want to dive deeper open 3d is at least one library you should look into maybe it fits your needs and you can form different types of mapping and map building using scan registration techniques out there if you want to look into further material read papers i have here a list of papers that i found useful which cover certain parts that we have discussed here in the different lectures um this goes to very early works like from arun and bessel and mckay proposing the icp algorithm or this svd-based alignment over more recent techniques overviews efficient variants of the icp comparisons of icp which metric is better in which situation so there's a large body of literature trying to come up with techniques supporting the icp better dealing with outliers adding redundancy um going for global icp methods so there's a large body of literature that you may want to dive into and then in the end maybe even use it to build up your slam system based on what you have seen here and again if you want to start with code jupiter notebook by igorgusluvsky or there's also a simple icp by philip agglira that is publicly available which you have in i think python julia c plus plus matlab octave so um all of starting points to play around with to start with a simple icp and then go deeper and with this i'm coming to the end so this was a three part lecture on point dot registration um looking into icp and its friends let's let's call it that way and it's an important task in robot perception in mapping that you need to be able to align point clouds either by shifting the sensor location and have the point clouds align and sometimes even going further and optimizing the point cloud positions it's on the point clouds itself or the positions in the point cloud itself icp or the iterative closest point algorithm is the standard choice so whenever you want to build a new system you will always have to compare yourself against icp and probably different variants of the icp for performing incremental scan matching for aligning point clouds and what it basically does it computes a translation and rotation between point clouds or scans or surfaces depending what your underlying structure is and if you have a given data association that's easy if you don't have data association then it can be tricky and the data association determines your translation and rotation parameters and the major problem is actually determining the correct data association if you know your data station perfectly it's easy and getting the data station right which includes finding out which points to trust how does the alignment look like which points are outliers which should not take into account which belong to moving objects if we have all that solved that's easy but determining this is typically tricky and what happens in practice is that you have an iterative approach performing the data association then the alignment then again the data cessation then the alignment and so on and so forth and there are several variants how you can do this and in the most general form you end up using a non-linear least squares problem and again your initial guess is always helpful especially if you go for the least squares approaches you will not be able to get very good results without an initial guess unless you can make very strong assumptions about your theme which then is similar to an initial guess and what you often find if you look to more sophisticated algorithms is there will be a least squares approach probably being used there's probably some point to plane metric or plane based metric being used there is some data station heuristics involved in maybe looking into which points are compatible to have a good starting point with and then also some outlier rejections which are implemented in here so this is kind of the standard ingredients that you find today in popular slam algorithms and with this coming i'm coming to the end and i hope you find this video useful and if you want there's also a minute summary on the iterative closest point algorithm that i recorded within my five minutes with soul series if this was all too long three hours of icp you can just enjoy this five minutes so thank you very much so icp stands for iterative closest point it's a technique in order to align to point cloud with each other so what means point cloud alignment a point cloud is typically a set of points of 3d or in 2d and what we want to do is we want to compute the transformation between two point clouds and the goal is that through this transformation points that are the same point in the real world actually lie on top of each other so that we can incrementally build a consistent model of the environment and therefore that's a key building block of most slam algorithms for example those that work based on laser range finders or rgb cameras so how does that work let's dive into an example and understand how this icp algorithm works it basically has two steps a data association step and then a second step computing the transformation based on that data association so the first step let's say we want to align the blue and the red point auto scene over here and we do this using a simple nearest neighbor approach that means we iterate over one point cloud and look for the closest point to these points in the second point cloud and then make this alignment and these are my correspondences my data association that i have guessed based on this initial configuration the second step takes this data association and tries to find the transformation so that those two point clouds are moved on top of each other so to say so that the distances of those corresponding points actually get minimized and this is done using two steps the first step computes the center of mass of those corresponding points and shift the point clause on top of each other and the second step computes the optimal rotation using an svd based approach so that the points then are better aligned with each other why better and not perfect because our initial data station may not have been perfect so what we do is we repeat this process and based on the on the on the guess that we have right now how the points clouds are aligned we recompute our data associations and then recompute the alignment and we do this over and over again data association alignment data association alignment data station alignment until we converge and this is basically the vanilla icp algorithm the iterative closest point algorithm in its basic form in practice there are different variants out there how you can optimize the icp algorithm one prominent example is the point-to-plane metric here you assume that the points don't stem or are not distinct points in the environment but they actually stem from a surface they're basically randomly sampled through the scanner from a surface and by map making an alignment between the point cloud and the surface we don't create an artificial point to point alignment which may not exist in the real world and this point to plane alignment typically or the plane metric typically gives you a better result of your icp algorithm but what it does it requires that you leave this svd-based approach and you have to go to general least squares approach and then you would most likely use the gauss-newton method in order to minimize the error under this point-to-plane metric but there also several other variants out there such as techniques using a projective icp that means you take the model that you have and project it for example into the range image that you created from your scanner and then try to find the alignment and you may also want to take into account robust kernels in your error minimization approach not a better deal with outliers so if you have outlier situations where some of the points are really wrongly aligned then those outlier rejection techniques help you in order to weigh those points down and ignore them in your data association or reduce their impact when computing the transformation so there are several variants out there and it depends a little bit on the problem that you are addressing which of the variants you want to use but a point-to-plane metric is probably today the standard for aligning point clouds for example those that stem from a laser range scanner if you want to investigate the icp algorithm further i suggest visiting the jupyter notebook that has been created by igor bogusluvsky and that guides you through the svd-based approach to the least squares approach telling you how you can deal with outliers it's a very good starting point which you can grasp in a rather short amount of time how icp works and providing you with a very simple python implementation for this so i hope that was useful and you get an idea how the key building block of most laserwise slam systems work they are used in robotics in autonomous cars not to build consistent models of the environment whenever you have for example a 3d layer rangefinder mounted on your mobile platform i hope that was useful and you learned something

Channel: Cyrill Stachniss

Views: 7,176

Keywords: robotics, photogrammetry

Id: CJE59i8oxIE

Length: 63min 37sec (3817 seconds)

Published: Tue Mar 23 2021