So what we want to allow is a shift, a rotation, and a scale factor between those two lists. And this is called a similarity transform. So given a left point, we want to rotate this point, we want to apply a scale, and

we want to apply an offset, a shift, so that this point is moved to its right partner. Now the scale, this is a scalar value from R, the real numbers. The rotation, in this case in 2D, has four values. So it's a rotation matrix, and we may use the following matrix, which is well known.

And so we see the only parameter in this rotation matrix is the angle alpha. And the translation vector is a vector from R2. So overall, we do have the scale, lambda, rotation, alpha, and our translation vector, Tx and Ty.

And so we need to find four parameters overall. So in reality, the transformed left coordinates are not identical to the right coordinates because we have noise. So in reality, we'll have to optimize the difference between those two vectors, which is often done in a least squares sense.

So what we want to do is, we want to optimize this sum, which means we want to minimize the squared length of the remaining vectors between the left and right set of points after I have transformed the left set of points.

So now how can we determine this? Well, that's a scalar. But here, in R, as we know, we have those cosine and sine functions, and we have the translation. But this formula tells us that this is a nonlinear problem. So the typical solution would be to linearize and then iterate a search for a minimum.

And this would mean we need start values, and we have to iterate, and we don't know if we will find a global minimum. So this is why I show another method here. So first of all, let us compute the center of mass for each of the two point clouds. So that the left center of mass is one divided by the number of points times the sum over all points.

And the right center of mass is similar. And after having computed the centers of mass, let's subtract them from the original points. This will lead to reduced coordinates, and we will denote them by using a prime.

Now this essentially means that previously we had those coordinates in some global coordinate system. So the points interpreted as vectors look like that. Now after we computed the center of mass, our new vectors will look like that.

And of course now if I do the sum over all reduced coordinates, this will be zero. Because we moved the set of points so that their new center of mass coincides with the origin. And the same for the right set of points. Now let's go back to the original formula that we had to minimize.

Let's put in our new reduced coordinates. Now if we rearrange this, we obtain this lambda r l i goes here. And the r i goes here. And what remains is the second part multiplied out, the mean of r, and t.

So if we just denote that as t prime, then what remains is the following formula. So in order to minimize this original value here, we might as well minimize this sum which uses our reduced coordinates. So let's minimize this sum. Now this sum has a plus here, and so this reminds us of the binomial theory.

So if we apply this to our formula, we obtain this result. So this is the a, this is the 2ab, and this is the b.

The only tricky thing is, since these are vectors, 2ab is 2 times our translational vector transposed, times the vector that is obtained by summing this up. And so behind here, that is m times the squared length of the translational vector.

And here this sum gives us lambda, the sum of r l, minus the sum of r. But the sum of r, that is zero, because these are reduced coordinates, so the center of mass is in the origin. And the same holds here, because the rotation does not change the length.

So this is the same as lambda r, the sum of l. This also is zero. So the entire term here is zero. And so what remains is this term, which consists of the first part and the last part. And we still have to minimize this. Now if you look at this, this is the sum of squared values.

So this is greater or equal to zero. The same holds for the second term. Now if I have a sum of two variables, which both must be larger or equal to zero, then I may obtain the optimum by selecting them being zero. So in the second case I can do this. T prime here is not part of the first term, so I can minimize the overall sum by just selecting T prime to be zero.

So T prime being zero means, according to our definition of T prime, that lambda rl minus r plus T, our original T, is zero. And so we obtain T as the center of mass of the right points

minus the center of mass of the left points, which are rotated and scaled. So we already obtained T, and all we have to do now is to obtain r and lambda. So what remains to be optimized is this term.

We don't have to worry about translation anymore. All we have to do is determine the scale and the rotation. Now we'll do this multiply out trick again. Now this looks like a minus b squared, which is a squared minus 2ab plus b squared. And I will do now a trick.

So instead of optimizing this, I will optimize the following. Which means that instead of scaling up the left point cloud and leaving the right as it is, I will scale up the left point cloud only by a factor of the square root of lambda.

And I will scale down the right point cloud by a factor of one over the square root of lambda. So this makes sure any error that I have in l and r is treated symmetrically. So let's multiply that out. So by using this theorem, I obtain lambda times the sum of the squared length of all the left vectors after they are rotated.

And as we know, rotation doesn't change the length. So this is the same as the sum over all squared lengths of the left set of points. And the second part, which does not involve the scale, and the third part.

Now my formula is of the form lambda times sum a plus b plus one over lambda c. And it turns out, to minimize this, I have to select lambda square is c divided by a. And b, this term in the middle, does not play a role. So in our case, this means lambda square equals the sum of the squared vector lengths of the right point cloud divided by the left point cloud.

Or lambda equals the square root of the quotient of the squared right vector length divided by the squared left vector length. So this is our second formula. And remember, since the b here didn't have an influence on the value of

the scale, we can determine the scale here independently from the rotation and the translation. Now all that remains is this part here. So in order to minimize the sum, as there is a minus here, we have to maximize this part here. So now the task is the sum of the right transposed vectors times our rotation matrix times the left vectors should be maximized.

There is no translation anymore. There is no scale anymore. All we have to do is, we have to find the rotation matrix. Let's multiply this out for a single point. Let's just drop for a moment this primes.

We have rx, ry transposed as a row vector times the rotation matrix times left x, left y as a column vector. And if we multiply this out, we get this vector times rx, ry. Let's multiply that out too. So overall, we get this scalar value.

I see it's a matrix multiplied from the right by a column vector and from the left by a row vector. So we get a scalar. And now let's group this. So we have a cosine times rx, lx plus ry, ly. And we have a sine times minus rx, ly plus ry, lx.

Now let's remember we have to sum that up. So our overall sum, which was the sum over ri transposed times rotation matrix times li. Now it's the same as cosine times the sum plus the sine times the second sum.

Now we have to find the cosine and sine so as to maximize this entire sum. But this is really easy because this is a cosine sine. That's a unit vector multiplied by those sums.

Let's think about this. This is a vector. And we're looking for this unit vector here that maximizes the scalar product with this vector. You see, the scalar product is this times the length of the green vector. And we need to find the unit vector. And so as it's easily seen as the vector that maximizes the scalar product goes in exactly the same direction as the green vector.

And so we find out that the cosine and sine we're looking for, this is just a vector here. Divided by the length of the vector. So we have to take this vector, build the norm and just divide by the norm of this vector.

So this is the solution to the third part. So in order to find the cosine and sine of my angle, all I have to do is build those sums and normalize the two values by the length of the overall vector. So if I want, by using that identity, I can recover the angle alpha.

So here's the recipe. You are given two point sets, left and right. Point numbers, say, between 1 and n. Then compute the following. Compute the center of mass for the left and the right point set. Then compute reduced coordinates, the l being lx, ly and the r being rx, ly.

And then set up the following sum. For the index running over all points, compute the cosine sum, which is the first element in the vector that we just built, and the sine sum. And also compute the lengths of the vectors, which is actually the squared lengths of the vectors. Of course all sums starting at 0.

Now use all the formulas that we had to compute the scale, the rotation and the translation. So the scale equals the square root of rr divided by ll. Remember the Bose squared length summed up for the right and left. That's exactly what we're doing here. This is the first result. And the cosine and sine, which we just defined, well this is just the cosine sum and the sine sum divided by their normalizer.

So this is the rotation. The translation, remember this was r minus lambda rl. This is the right center of mass minus lambda times the rotation matrix, which is

cosine sine minus sine cosine times the left center of mass, which is the third formula. And as you see, to compute this, I need the right center, which I computed right in the beginning, and the left center, which I computed right in the beginning. I need the lambda, but I computed that already here. I need the rotation matrix consisting of the cosine and sine, which I computed here.

So all I need to know is there. So this is our third formula. And then the function you'll have to implement, it should return a tuple made of the scale, the cosine, the sine, and the translation in x and the translation in y. I just see I forgot all the primes here, so don't worry. So sometimes, instead of a similarity, you are interested in a rigid body motion, which means that you don't want a scale factor to be applied.

So you want lambda to be equal to one. And you can achieve that easily, because instead of computing lambda this way, you can just replace this by lambda equals one. So in the final implementation, I asked you, depending on the flag, just don't compute this value,

but instead set lambda equals one, and we'll still estimate the best transformation in the least square sense. However, for a rigid body motion instead of a similarity transform. So now let's implement this. So I prepared some code for you, which is SLAM4C estimate transform.

And it is based on the SLAM4B code. So first of all, this function finds the cylinder pairs, and that is what you have implemented in your previous code. So just insert your previous code here. And then down here, there's the function estimate transform, which you should implement. And it gets a left list, a right list, and a flag, which is called fixed scale.

And if this is true, then you should set the scale factor, your estimate, to one, instead of estimating it using the formulas from the previous slides. So as you remember, in order to estimate the transform, you need to compute the center of mass for the left and right list. And I implemented that for you already.

So this function computeCenter() gets a point list, just sums up the x coordinates and the y coordinates, and then returns the sum divided by the number of elements. And here I called this already, so you can just use those two lines. And after you compute everything, remember to return a tuple of five values, which is the scale factor lambda,

the cosine of the rotation angle, the sine of the rotation angle, and the translations in x and y. And this estimated transform should turn the left list into the right list, and not the opposite. Down here I implemented a function which applies a similarity transform. So given the transform, which is a tuple made of scale cosine sine tx and ty, this function will transform a point

P by just applying the scaled cosine and sine to the coordinates and adding the translation and returning the resulting xy tuple. So in the main function, only a few things have changed down here.

So we still update the pose, we compute the coordinates of the cylinders in the scanner's coordinate system, transform them to the world coordinate system, then find the cylinder pairs. Then here we call your new function estimateTransform() which gets the world cylinder points and the reference cylinder points. And these list comprehensions here, they are made to pick only the points that are part of a pair that was found in cylinder pairs.

So if you have six reference cylinders, and maybe you found four world cylinders, but then in these cylinder pairs you only get three pairs because the fourth one didn't find its partner. Then this constructor will build two lists with three points each.

And here I'm setting the fixed scale to true because I do not want the transform to scale the result. And after you find the transformation, we transform the world cylinders using that transformation so that we can see the final position of the detected cylinders in the world coordinate system after applying the transformation that fits them best, in a least square sense, to the reference cylinders.

So now try to implement this function up here. And don't forget to insert your code to find the cylinder pairs from your previous solution.