ICP & Point Cloud Registration - Part 1: Known Data Association & SVD (Cyrill Stachniss, 2021)

welcome to our first lecture on point cloud registration so i want to dive today a little bit into the details of the iterative closest point algorithm or at least starting with a mathematical basis of the icp algorithms so we will look into the question how we can align 3d or 2d point clouds with respect to each other and that's a key ingredient that we use in scan matching approaches for example to build maps of the environment and this is the first part of a lecture there's a second part in the first part we will look into the mathematical details on how to compute an optimal alignment given point-to-point correspondences and in the second lecture we will then relax the assumption that we know the data of cessation or the point correspondences and we'll estimate them while we are iteratively estimating the correspondences and compute the transformation itself so we start today with assuming known correspondencies so we know which points in point cloud number one correspond to which point clouds in point cloud number two okay but let's start why should we actually care about scan registration or alignment we should care about this because it's an important ingredient in mapping applications for example so if you have a platform navigating through the environment equipped with a sensor that records the 3d scene so this could be a laser range scanner this could be a depth camera or could also be a stereo camera this sensor will generate data range data about the environment and this range data will generate 3d points in the world for example where obstacles or where there are distinct points or landmarks in the environment and while we're moving through the environment we need to register those individual readings so put them into the same reference frame and this is something that we do here in point cloud registration where we want to find the transformation so that two sets of points will overlap as good as possible so for example you have a small mobile robot navigating through the environment and every few meters it takes a range scan moves forward takes another range scan we need to align those range scan in order to get an accurate model of the environment so here's just a small illustration of a surface and same we can do also with surfaces and we have two scans or two readings of the surfaces the gray one as well as the yellow one and if this is an initial gas so the initial two surfaces put into the ram same reference system we can see that they actually don't align very well and what the algorithm that we are looking into here today is they try to find a transformation update the transformation so that those two surfaces or those two point clouds are put into the same reference frame and again this is something which is commonly used in robot mapping or slam algorithms it's basically the first step to register one range reading against the next one here's an example is a car navigating through the environment equipped with a 3d lidar so it generates 3d point clouds 10 times per second and the task is to register those point clouds in order to estimate where the car was while navigating through the environment and by concatenating those point clouds appropriately we can actually build maps of the environment and scan matching or scan registration is the first step in this process of building a map of the environment and that's what we want to investigate here today so let's see what means registration of 3d points so we assume we have two or more sets of points so today we'll only focus on two sets and those sets have typically been recorded at different locations so while for example the platform was navigating through the environment and the task now is to find the transformation typically a rigidbody transformation that means a translation vector and a rotation matrix so that these two point clouds will be transformed into the same reference frame so that the same position seen in scan number one and seen in scan number two will actually correspond to the same three location in the world so that means we are trying to find a rotation matrix and a translation vector so that we can align those two point clouds we assume those point lots to be rigid so not deformable at least initially so that we don't need to need to deal with deformations inside the point cloud so something that we ignore for now and there are two ways or two basic principles how i can do this task of registering those two point clouds the first one is an svd-based procedure and that's the procedure we'll look into today which is the basis for the iterative closest point or icp algorithm which tells you given correspondences how you compute this transformation so this is something that we refer to as known data association or known correspondences and this is what we will be focusing here in the first part of the lecture so we assume which point in point cloud number one corresponds to which point in point cloud number two not all points need to have a corresponding partner but we need to have a sufficient number in the second part of the lecture we will then relax this assumption and try to estimate the correspondences as well but for today we assume known correspondencies and if we have those correspondences we can perform this alignment so if we are coming back to the example that i've shown before here with these two surfaces then what we're basically doing every step so every update of this algorithm is one of those alignment steps and in all those steps we assume the data station to be given and then compute this relative transformation and so we're just looking into this single step on how to compute this transformation so the part one of this lecture focuses on point out registration with known data station or with known correspondences and we want to derive a solution on how to compute these transformation parameters in an optimal way with a direct solution and also an approach which is fairly efficient to be executed under the assumption of course of known data stations that is important over here in the second part of the lecture we will then relax this assumption but for now we always assume non-correspondences so that means we have two sets of points we have a point cloud y and the point cloud x consisting of different numbers of points and we assume to have this correspondence is given so the correspondences can be expressed with a set of pairs of indices i and j for example telling me that point number one in point cloud y corresponds to point number 10 in point cloud x for example so this is what these correspondences actually tell me and what i want then i want to find i want to find translation vector and the rotation matrix so t and r so that i can transform one the point cloud x into the coordinate system of the point cloud y so that the euclidean distances between the corresponding points in x and y gets minimized i'm actually minimizing the squared error over here for all those corresponding points so basically have a sum and summing over all corresponding points say where is the location of this point in the point cloud y i subtract from this the location of the point x transformed into the reference frame of point cloud y i square that arrow and that's the term that i want to minimize so if i can minimize this expression i found the optimal rotation matrix and the optimal translation vector that aligns those two point dots for me if i have zero noise and i scan exactly the same points the sum can theoretically be zero in practice this is often not the case because of course we have noisy measurements about the world and we cannot guarantee that we always measure exactly the same point in the environment although we assume that to be here the case for our correspondence problem but that's nothing we can guarantee so this term is typically not zero but we want to make it as small as possible so the key idea is that given those correspondences um i'm able to find the rotation and translation in one go with the direct solution that means i don't need any initial guess so assuming i have the surface here the blue surface and the red surface and i have a few correspondences here indicated by those lines i want to find the rotation matrix and the translation vector so that those two overlap in this example here the red curve is transformed into the blue curve and that's the task that i actually want to look into before we start i want to simplify the notation a little bit especially those correspondencies so i'm not interested in all points in both point clouds i'm only interested in those points where they exist the corresponding partners because the others don't contribute to the underlying estimation problem over here so i'm basically re-ordering those points in x and y in a way that first i consider only those points which have a corresponding partner and then order them in a way that the first point in point cloud x corresponds to the first point in point cloud two and the second point in point cloud x corresponds to the second point in point cloud y and so on and so forth so that i basically have an index n which i can use to index a point in x as well as in y and i can ensure that this is a corresponding point so this is just reordering basically the subset of corresponding points in both point clouds that doesn't change the problem itself but it makes the notation easier and more lightweight so that it's hopefully easier to understand so from now on i will not use this set c anymore i will just use this index n and so that the variable index with n in the point cloud x and point dot y corresponds to a pair of points and then i want to find the rigid body transformation so that i can transform the points in my point.x my input point cloud so they will be all transformed to a new set of coordinates called x prime and this should be done in a way that the sum of squared errors the point-to-point errors that remain between x bar n and the corresponding point y n is minimized so that's our overall goal and the problem that we are looking here into is actually a special case of the so-called absolute orientation problem that is frequently used in geodesy also in computer vision which is actually a more complex problem to the problem we are looking in here where we try to find the similarity transform between two point sets so in the absolute orientation problem we don't only have rotation vector in the trans rotation matrix and translation vector we also have a scale parameter for example if we performing a 3d reconstruction from monocular images and we don't have the scale information then we would need such a similarity transform here for the point cloud alignment we assume that we know the scale so we don't need that scale parameter this scale parameter is basically assumed to be one in all the things that we are doing from now on here so that this reduces to a rigid body transformation that i'm considering here but just to make clear this is a special case of a more general problem and then we want to start diving a bit more into the details and actually derive the solution itself so the formal problem definition says we have a set of corresponding points x and y n and we maybe also have weights so that means all of those correspondences have a weight for example how certain and i am about this correspondence that's something that i just added here if you have perfect correspondences uh we typically don't need those weights then we have equal weights all the weights are equal to one but if i later want to integrate for example uncertainties then these weights become handy come handy so the derivation here also uses those weights p n and i want to find the transformation parameters again so that the weighted sum of squared errors gets actually minimized so all of those squared errors here have a weight associated to that in this example over here and the important thing to note in here is that a direct solution exists direct solution means that it's not an iterative procedure where i need an initial guess so completely free of an international gas i can actually come up with a rotation matrix and a translation vector that minimizes this term and it's also the optimal solution that means there exists no better solution and i don't need any initial guess which are two great properties that i have in here again this always assumes perfect data cessation so we need to we can't guarantee this if we have don't have the data station or don't know about the data station so informally what this algorithm does this approach does that minimize this function it basically involves a shift of those two point clouds and i'm shifting them together basically in a way that i'm aligning the center of masses of those points so basically shifting those point sets so that their center of mass overlaps and then i'm performing a rotation that minimizes the error not changing this rotation point anymore so these are kind of informally speaking the two steps that we are doing here computing the center of mass is fairly easy i just need to sum up the points and average over them the second part here estimating the rotational component is somewhat more complex and involves a singular value decomposition that i need to execute in order to find the optimal rotation matrix okay so this direct solution says i need to compute the mean or in this case weighted mean of my point that y and my point set x which is shown over here so indicated as y zero and x zero so it's just the weighted sum of the points itself and then i'm using those weighted sums and compute the so-called cross covariance matrix so i'm taking the points said y subtract the mean from the points and do the same in x and then i compute the outer product of these vectors and summing all of them up together with the weight and this gives me this cross-correlation matrix h over here and then i'm taking this matrix h and perform a singular value decomposition of h so i'm decomposing h into three matrices into a matrix u into a matrix d a diagonal matrix and into a matrix v or v transposed over here so the svd is a standard mathematical procedure which decomposes these matrix h into three matrices with special properties and then i can compute my rotation matrix as v times u transposed so the first and the last matrix and that result from this svd and this gives me my optimal rotation i haven't explained you why this is the case it's something that we'll do in a couple of minutes i'm here only showing the solution what needs to be done so i'm computing the center of masses i compute this matrix h compute the svd and from the result of the svd i can extract the optimal rotation matrix and we can do this in a similar way for the translation vector here we again use the center of mass of the point clouds of x and y and then we have the rotation matrix that we just computed in the last step and then the translation vector is simply given by um y 0. so the center of mass of the points at y minus the rotation matrix times the center of mass of the point set x so the weighted mean of this point set x again i haven't explained why this is the case i only provided you with the solution but we will derive the solution in a minute so that the result of this algorithm basically says okay you can compute your rotation matrix r and your translation vector without an initial guess as the optimal parameters given in this form using this matrix h the svd and the center of masses and that's what the solution to the algorithm is so if you let's say you don't care why this is the case you only want to compute r and t you can stop watching this video and just write down those quantities and you're done with it if you want to understand why this is the case and actually derive everything from the basics then you should stay with us for a bit more time so the svd-based alignment if i break it down into steps say i come compute the means of those point sets and y 0 and x 0 then i can compute something which called mean reduced coordinates so i'm basically subtracting the mean from the point that y and the mean from the points at x and then i can compact compactly write this cross covariance matrix um that i compute this matrix h then i look into my mathematical toolbox and use my svd function which is in there which again decomposes this matrix and from the result i can construct my rotation matrix r as well as my translation vector here as we described it before and then i can simply transform the point set x with this rotation matrix and the translation vector which gives me x prime and which is the other transformed coordinates and then the sum of the squared differences between y n and x bar n is minimal so um in the end you can see this as a translation of one point sets onto the other points it's so here moving the red one onto the blue one and we're basically shifting them so that their center of mass overlaps and then we are basically performing a rotation around this center of masses so that the points are then properly aligned with each other and so i can express this here also in this form of the equation but that's basically what happens and if i know my correspondences again this is the right way to do it and we are done no further steps are needed for known correspondences so done we are happy nothing else needs to be done if you are someone who wants to dive a little bit deeper and actually understand why this is the case then you should stay with us and because you probably ask yourself the question why is this a good solution or not even a good solution why is this the optimal solution why is there no better solution why do i not need an initial guess although some people may have told you you need an initial guess for the icp algorithm if you want to learn that then stay with us and we will actually go back and start from the beginning and derive the solution for computing the matrix r and the translation vector t over here so in the next let's say half an hour we will dive here into the details okay so let's start again as a reminder my formal problem definition which was the points that y x and my weights and we want to find rotation matrix in the translation vector so that the differences between the points in points at y and x bars so the transform points from x gets actually minimized so that's the task so the first thing we are doing now we are defining our new coordinate system and we are defining a local coordinate system in which we want to work and this the coordinates are defined through this point set y so the target points it where i which we should define the coordinates of my final point set and the origin should be the weighted mean of y of all the points y n so i'm summing over all points y and multiply them with the weights and then normalize through the sum of the weights um and i want to operate in this local coordinate system so all the transformations are now done in this coordinate system where the origin is the weighted center of mass of this point set y zero and if i look to the equation that i want to minimize then then the expression gets more complicated so i get this this poinsett here and this y zero over here but the important thing to note that this doesn't change my problem because i'm basically adding this y zero and um or subtracting the y zero and adding the y zero again so it doesn't change the overall minimization problem that i need to look into and so basically from now on i want to compute everything in this coordinate system where the origin is the weighted mean of these points y n okay so the second thing i want to do is i want to rewrite my translation vector a little bit so again i had my transformation here and i want to do some transformation now with this equation and to rewrite t so it looks slightly differently and this will become handy later on so what we're doing we're subtracting the mean of the points at y 0 from both sides because we said we want to work in this mean reduced coordinate system i'm subtracting the mean of the points that's y n here so we're basically taking this equation and subtracting y 0 on both sides and what i'm then doing i'm basically rewriting this expression over here exploiting the fact that a rotation matrix times the rotation matrix transposed is the identity matrix so i can basically add this bracket over here and multiply r transposed to the two quantities here which were originally not multiplied with my rotation matrix and so then i have this expression over here and in this different local coordinate system i can define kind of a new vector over here um based consisting of these quantities here which i now call x0 and it's important to note that at this point um i don't know what x0 is later on it will turn out that this is the weighted mean of the point set x but this is something i don't know at the moment this will be the result of the computations here so i'm basically introducing a new variable x0 which is defined like this so if i know r and i know t then i can compute x0 so i don't add kind of any new unknowns to my problem i'm just kind of rewriting this to simplify my life a little bit and i'm introducing this new variable x0 so that this is then my final equation over here so i move the translation basically inside the bracket which basically means this is the translation that i'm first executing subtracting from the point x n before i perform the rotation over here but again this is just kind of rewriting things and also this doesn't change my initial minimization problem the only thing i need to do i need to now work in this local reference frame which was basically i'm subtracting this y0 from xn what i did before and then i'm having here on this side basically replaced this vector t with my x zero over here and so it's still the absolute same minimization problem and the only thing i need to do now to minimize this equation i don't need to compute r and t i need to compute r and x0 so these are the two variables i'm minimizing and i want to compute this optimal rotation matrix r star and the optimal vector x 0 star that minimizes this expression so what i've done so far i've just rewritten my quantities a little bit to simplify my life later on and now i actually want to minimize this expression over here okay so what i do i take this expression and turn this into my objective function and basically just rewriting my objective function a little bit um so it's again it's a quadratic term and so this is the coordinates of the set y subtracted by their mean minus r times x n subtracted by my new variable transposed the vector again times my pm and so what i now want to do is i want to minimize this function and compute the minimum of this function and so the question is how to do this how do i minimize such a function and you should all know how you minimize a function this goes basically back to your courses that you have even taking at school how you have a general function f and you want to define the or compute the minimum of this function what you typically do you compute the first derivatives you check for the points where the first derivative is zero which is then a minimum or a maximum you're selecting the minimum and this basically boils down here to um solving the resulting equations that i obtain if i first compute the first derivative set it to 0 and then solve the resulting equations then i get the optimal parameters x0 and r over here and that's exactly what we want to do now so um in order to do this you know to compute the first derivatives i'm basically expanding this dot product over here or the vector transpose times vector and basically multiply the expression with you also this expression times this equation this expression times this this times this and this terms itself which gives me three individual sums with those individual terms over here so i'm just kind of rearranging my objective function a little bit i don't change the quantities itself so nothing really magically has happened over here and then i can actually look to these three lines over here first line second line third line what you can see here in this first line is that is actually an expression which doesn't depend on x0 and which doesn't depend on my rotation matrix r so this is basically a constant under those two parameters the second line over here only contains x0 but doesn't contain the rotation matrix and the third line contains both of those quantities so this expression no x0 no r pops up and here no rotation matrix r pops up and this is just a side note we will now continue with this objective function over here and compute the first derivative with respect to x0 set it to 0 solve it and then minimizing the function with respect to r as well so let's start with x0 because this is the somewhat easier part we want to solve this or minimize this with respect to x0 that means computing the first derivatives of this function with respect to x0 sending it to 0 and then resolving the equation so what we're doing is we are taking our objective function just copy paste from the previous slide and compute the first derivative with respect to x0 so we're taking our function phi our objective function and deriving it with respect to x 0. so the first term over here is as a constant because it doesn't depend on x0 the second term here is basically a quadratic term in um x0 so this gives out this 2 the term x0 has a negative sign over here which brings us a negative sign over here and then we have the sum and just taking to count the linear part over here and for the second part over here there this is only linear in x0 so we have the negative sign will pop up here we'll compensate for this negative sign over here so this turns into plus the two stays the same and then have this linear term over here um so this is directly the computation of the first derivative of this function over here okay now i take my first derivative and need to set this first derivative to zero right because i want to find the parameter where the first derivative is zero because that's the point where i have a minimum or a maximum okay so how does it look like i take this equation over here and simply set it to zero set the first derivative to zero so this is my resulting equation so what i can do now is i can basically take this negative part over here move it to the other side i can divide by the factor of 2 and then i'm adding and i'm getting this equation over here so this is this sum over here sits over here and this sum basically sits over here not much has happened okay and now i need to look a little bit into the details in order to um find the term which brings this generates this equality and i'm looking here to this sum so the sum over all points x0 xn minus x0 times the weight and actually i can ignore or i can't ignore the weight over here i need to take this expression now into account say what does this mean so this means nothing else that i could just sum over my points x0 and this is actually the the summing over the points multiplied with the weights actually gives me the mean of this point set and then i'm actually subtracting the mean of that points so i'm basically taking all the individual points subtracting the mean and then computing the mean over this expression this means the result of this sum must be zero because again i'm taking the points subtracting the mean from all the individual points and then i'm i'm summing over those points all these deviations from the mean and as a result of this i will end up in 0 because i've subtracted the mean before so this expression over here turns out to be zero and if this is zero a rotation matrix multiplied with zero vector gives me zero so that means this expression over here also must be equal to 0. so this is a result of that okay now i take this expression and continue from this expression over here just expanding the sum so i get this result over here and then i can um simplify this and move x n to the other side because x n doesn't depend on the sum so i can move x 0 doesn't depend on the sum so i can move it out of the sum divide by the sum term and then i'm actually ending up with this expression over here and if you look to this expression we can actually see that it is the weighted mean of all the points x0 and the important thing to stress in here is that this is a result of the fact that we want to compute the x0 that minimizes my objective function so x0 turns out to be the weighted mean of the point set but this is nothing which i have set manually or provided this is something which is a result of this minimization and that's also the reason why i call this variable x0 but when i'm starting when i was introducing this variable i was not knowing that it's actually the weighted mean of the points at x so this is a result of this minimization so the optimal value for x0 given my data association is the weighted mean of the point sets of xn so something i can compute easily just need to iterate over my point set x n over all the values sum them up multiplied with the weights normalized by the weights and then i'm getting the weighted mean the center of mass of the weighted points and x0 and this is my x0 which is one of the quantities from which i can then directly derive my translation vector because there's a direct relationship between x0 the rotation matrix and the translation vector okay so that's done i know how x0 must look like and it's easy for me to compute it's very good first part of the problem is solved now what i want to do is i need to take care about the second variable that i want to identify this is my rotation matrix rt so we did it for x0 for the vector now we need to do this for the rotation matrix we need to find the optimal rotation matrix so that it minimizes my objective function that's the task that we now need to look into so if i look to my objective function again copy paste from the expression that i had a few slides back and we had seen over there that the only the last expression here that third line is an expression which depends on the rotation matrix r so those two quantities over here are independent of r that means in my process of trying to find what how should r look like in order to minimize this expression i can ignore those two lines because they are completely independent of r so the only thing i need to take into account is this second line over here and i can say i either need to minimize this expression over here which is equal to maximizing this expression when i'm actually just changing the sign and i want to do this under the constraint that r transposed r must be an identity because we have a rotation matrix over here so this is one of the points that i will be exploiting also later on okay so let's see how that works um so given that we already computed x0 we can actually actually exploit that knowledge about x0 and we use this x0 just to rewrite things a little bit so that the expression get a little bit easier and we compute the so-called weighted mean reduced coordinates so we're taking our point set y and subtracting the weighted mean of y from them and we do the same thing for the points at x so we're taking the points computing the weighted mean of the points at x 0 and subtract it from all the points and so that basically um the the center of masses of those points overlap on r and 0 0 0. and so i can just take all my points y n i call them a n and just by subtracting the the weighted mean of that point set and do the same for x so basically coordinates x turn into b's and the coordinates y turn into a's just that there are weighted mean reduced coordinates so the weighted mean lies in 0 0 0. so then the expression simplifies basically those expressions are directly explained replaced with a and b and then this is the expression i'm actually need to focus on so how are we going to do this the first thing we are exploiting is that we can actually rewrite this expression over here which was exactly what i copied before by using the trace and we are doing this because later on we can exploit an inequality in order to perform this maximization that tells us something about the trace of matrices and therefore we are now shifting from this notation into the notation of using the trace and basically says that this expression is equal to this one over here so it's the trace of this rotation matrix r over here and a matrix h which is the cross covariance matrix of these vectors b and a so it's a n times b transpose so the outer product over here times the weight vector summed over all points and this is the cross covariance matrix of the vectors a and b and this gives me the matrix h and then this expression over here the original expression is equivalent to this expression so we basically need to find the matrix r that maximizes this trace so task for now is find r so that this expression here is as large as possible the trace of r times the cross covariance matrix defined over the individual data points so how do we do this and we are doing this by exploiting the singular value decomposition now again this is something where we basically say okay let's take the thing of the value decomposition decompose this matrix h and define a matrix r and then see what happens then we can actually show that the choice of r is an optimal choice so that's basically how this derivation works so we are taking this matrix h and they're computing a singular value decomposition of this matrix h that means we are breaking up this matrix h into three matrices a matrix u a matrix d and a matrix v or b transposed and here the important thing is that d is the diagonal matrix so zeros everywhere only elements on the main diagonal and these are the um the singular values of d is done in a way that these are positive values and u times u u transpose times u of the identity and v transpose times v is also the identity so these are the properties that hold based on the singular value decomposition okay and what we're now doing is we're performing this singular value decomposition and now define a matrix r using this u matrix and this v matrix with the goal of then showing that this was the optimal choice using an inequality so we are taking the results of the singular value composition say let's see what happens if we define r in the way that it's v times u transposed if we do so then we can actually rewrite this expression of the trace because now we know what r looks like so r is v transpose a v u transposed so v u transposed is r and h i just computed with a singular value composition as u times d times v transpose so this is my matrix h so if i look to this multiplication of five matrices i see here that i have two matrices in the middle the u transpose and u which must be the identity based on the singular value decomposition so this expression simplifies to the trace of v d v transposed so far so good the next thing i'm going to exploit is that d is a diagonal matrix which has only positive elements on the main diagonal and so i can basically break up this matrix d in a product of two matrices where on the main diagonal are the square roots of the singular values you just then need to multiply this matrix with itself to get d so d is split up into let's say d square root times d square root where d square root is basically the same diagonal matrix except that i have the square roots on the main diagonal so if i multiply those two matrices i will get the same expression why are we going to do this while doing this i actually want to replace this v square root of d with a new matrix and then exploit certain properties of this matrix therefore i'm doing this split up here which seems to be rather arbitrarily at the arbitrary at the moment because then we can exploit that further down the line okay so as i said we split up this matrix d as it is a diagonal matrix um we can rewrite this and basically move the transposition operator inside because again it's just a diagonal matrix so if i could be the transposition of a diagonal matrix it's the same so we can exploit this for splitting up the product as the trace of v square root d times v square or d in brackets transposed right this only holds because i have this diagonal matrix over here so i can now define a new matrix a as v times square root of d so that i can write the trace of r times h is equal to the trace of r a a transposed and so far this is just a definition just all these expressions just hold based on exploiting the properties of the singularity composition there's no other things have been exploited other than the properties resulting from the svd i furthermore know that this matrix a is a positive definite matrix and this again results from my singular value decomposition and that generated me this matrices v and d and so as a result of that i know that this is a positive definite matrix a and with a being a positive definite matrix i can exploit an inequality and this inequality actually tells me then that it i use the optimal rotation matrix so this inequality tells me the trace of any positive definite matrix a times a transposed is larger or equal than the trace of the same matrix multiplied with an arbitrary rotation matrix right so you can multiply any further rotation matrix here called r prime and then the trace of this positive definite matrix will only decrease or state the same value and this holds for any rotation matrix r and this is a result of the schwarz inequality and if i use this result i can say okay my trace of r times h is the trace of a a transposed which must be larger or equal than the trace of any arbitrary rotation matrix multiplied with a a transposed and now i can actually replace back that a transposed by design was equal to r times h so this is my arbitrary rotation matrix r prime times r and h and what this means here this expression is that i can multiply any arbitrary rotation matrix here from the left hand side to my expression which turns the two r's into a new rotation matrix and these new rotation the result of the trace then will always be smaller than my original trace so that means if i use any other rotation matrix then the rotation matrix that i have chosen the trace will be smaller that means this is the maximum trace that i can actually get for this type of matrix so as a result of this by choosing r smartly namely as v times u transposed this expression will be maximized and any other design of a rotation matrix will lead to a smaller trace and as a result of this this is the design of the rotation matrix that maximizes this trace if you want to go a little bit further and actually know why this inequality holds you can actually go back to this 1978 paper of arrow net al where they actually derive all that i just put the corresponding expression here on the slides that tells you why this term actually holds so as a result of this we now have defined the x 0 value that maximizes or the x 0 value that maximizes my objective function and i have found the rotation matrix which maximizes my objective function so this rotation matrix r equals v times u transposed is the optimal rotation matrix in order to maximize my objective function so as a result of this i have now computed x0 and r the two variables on which my objective function depended in order to minimize it so sorry if i said maximizing a second ago of course i mean minimizing because the oval function is i'm seeking to minimize over here so the question i may have is this a unique solution um it is only unique solution if the rank of this matrix h is of full rank so this matrix h is a three by three matrix if we're in the 3d world this same holds for u and v which are rotation matrices and so my matrix d is a three by three diagonal matrix with elements on the main diagonal and only if all those values are greater than zero this solution is actually a unique solution otherwise it is not a unique solution as soon as one of those values is equal to 0 then i don't get a unique solution out then there are multiple possibilities how this matrix can look like but otherwise it's a unique solution over here okay so i know x0 i know r and then i can compute my optimal translation vector just based on the formula that i used not to define x zero so this was my starting point how i defined x0 it depends on the rotation matrix it depends on the weighted center of mass of the second point cloud of the first point cloud sorry and the translation vector t so i can simply move t to the other side by moving this expression over here multiplying from the left hand side miss minus r and as a result of this i can compute my translation vector t in that given form which is the optimal transaction vector for my overall minimization problem so what we have done now we derived the solution that minimizes my objective function or my error function and you computed the optimal solution coming up with the rotation matrix and a translation vector that doesn't require any initial guess and there's no better solution than the one we have proposed under the assumption of no data association the only steps that i need to do i need to be able to compute a cross-coverance matrix which is trivial i need to compute the weighted center of masses which is trivial and i need to compute the singular value decomposition for which i typically use my mathematical toolbox in order to do this so as a result of this with those parameters i have found the optimal alignment of these two point clouds there's no better solution than that it's optimal and i don't need an initial guess so it's a direct solution that we could compute so these are all great properties for such an algorithm that i want to have it's optimal i don't need an initial guess and furthermore this is also really fast to do because i'm only dealing with three by three matrices and the singular value decomposition of a three by three matrix if nothing which challenges your computer a lot so to sum up what have we done we have provided the registration algorithm for aligning points two point clouds under known data cessation so assume we know our correspondences it provides a rigid body transformation it's a special case of the absolute orientation problem so we could also do this if we have a scale ambiguity in here so if one point cloud needs to be scaled we can do the same then our objective function gets a bit more complicated we have another parameter in there but what we have shown here is basically a special case of that and um it is again a direct and optimal solution and a very effective and popular method so the standard icp algorithm that we will discuss in the second part of the lecture is basically based on this s-video solution so what we will be looking into the second part of the lecture is point load registration with unknown data association so there we relax the assumption that we know the correct one-to-one correspondences between those points and look into ways what can we do if this is not available in general as a short outlook it turns out that the that there is no direct solution that i can actually execute if i don't know the data station so i can maybe guess a data association like the one shown over here which is not the optimal one but then i will of course get an alignment which is not perfect because my data station was not the right one and so the trick of the iterative closest point algorithm is basically to repeat this process so based on the fact that in step one we're going from here to here let's simply try again re-estimate so guessing data associations and then recomputing this alignment as an iterative algorithm another we can basically repeat using the toolbox that we derived and iteratively executing is always guessing the data station again and this leads to the so-called icp algorithm so that after a couple of these icp steps we hopefully end up getting the right solution but again the assumption here is um we don't have perfect correspondences but we have a good idea how those correspondences may look like so we have an initial guess either based on the point locations and then we can look for example for the nearest neighboring points or we have an idea of point correspondences if you have no idea at all what the initial guess could look like we could still try to align the points with the computing the center of mass and trying to shift them on top of each other but then it typically gets more tricky and the key idea again is to iteratively estimate the data station and the transformation the data station the transformation until i actually converge and this goes back to an algorithm which is by now nearly 30 years old and is known under the name iterative closest point algorithm and it's one of the extremely popular methods that is used for aligning point clouds with each other and is the standard in a lot of slam algorithms that we are basically using today and again it works by through the idea of picking for every point its closest neighbor the rigid body transformation execute the alignment and repeat this process until nothing changes anymore and this actually brings me to the end of the lecture today what we discussed is the registration of point clouds which is a central task for building maps for slam algorithms for estimating the movement of a mobile platform incrementally something like ladder-based ladder-based odometry these are all techniques that rely on some form of point cloud registration and we looked into a technique that is used in order to compute the optimal alignment in forms of a transformation and a rotation between point clouds or laser scans some range data that allows us to compute 3d points in the environment and given the data associations the svd allows us to come up with an optimal solution and this is the basic for the icp algorithm that we will then discuss in the next lecture and with this i'm coming to the end i and thank you very much for your attention over here thank you you

Channel: Cyrill Stachniss

Views: 11,631

Keywords: robotics, photogrammetry

Id: dhzLQfDBx2Q

Length: 52min 36sec (3156 seconds)

Published: Sat Mar 20 2021