Lie Groups for Robotics solving problems with Code

[00:00:01]Hi, this is um tutorial of L groups for robotics. I will explain L groups not by delving directly into mathematics and then trying to solve uh problems but the other way around. So I will go directly into the problems and then I will uh apply le groups to solve them. uh I will also attach code to it.

[00:00:30]So there will be a repository which has this URL where those two problems are also resolved in code using pipos.

[00:00:42]Okay. So the first problem will be pose interpolation. So in the problem of pose interpolation you have two input frames.

[00:00:50]One is the initial frame that is represented by TWWI. So the initial frame in world coordinates. So how world the world sees the initial frame and then the other is the final frame and we represent it with this transformation.

[00:01:10]That is how the world sees the final frame. The output is one or more in this case you can see two in interpolated poses. So poses that are between the initial and the final. In this case, we have the pose one are represented by the transformation TWW1 and the post to represented by the transformation TWW2.

[00:01:39]There could be more. There could be just one. The important thing is that our intermediate poses between the first and the last.

[00:01:50]The second problem that we will resolve is pose averaging. In pose averaging say that at t minus one. So the previous time of step we have an estimate as the previous pose which will be t minus one.

[00:02:05]This one uh we will utilize this transformation t to t minus one. So how the world sees the previous pose t minus one. And then we will assume that we have different sensors. We have we will assume that we have inertial sensors that will provide us this um frame O then we have a camera that will provide this frame seal. So let's say that we have very expensive 3D camera and we get this frame and then we have some maybe foundational um geometric models and provides us this AI frame. Note that all these sensors provide transformations with respect to the previous frame. Okay. So the previous frame was t minus one. The current frames are those here. So the idea is that the real or good or optimal estimate of those frames will be something in between. So we will just average those. Okay, there is the possibility to do weighted average which is in the code but now I will explain just the average for the sake of simplicity. And by averaging those we will get this thing here. Okay. And we assume that this is the current frame.

[00:03:33]And note that this is represented uh from the transformation of t -1 to t.

[00:03:41]Okay.

[00:03:43]So as uh previously I said that I would not go into mathematics and then try to solve the problems but there should be a very basic at least um baseline. This is not uh explaining mathematics of transformations but this is just making sure we are all in the same place and we all understand the same when we mention transformation so it is uh clear okay so I will explain the SC3 group but this is more a review of something you should already know okay so let's say we have a frame I and let's say we have a frame and we want to see how the frame I sees the origin of the frame F. Okay. So for that we will use this transformation. So transformation from I to F which means how I sees F.

[00:04:40]And for this tutorial I will use mostly the representation of the 4x4 projective matrices but um in principle for all the league groups is representation agnostic. So um all the operations that we'll show in the future has its own implementations in regards to the um the uh implementation of the SE3 group which implementation of S3 group is called usually representation. Okay. So you have you can have quatians per plus transformations uh translations you can have dual quaterians but we will use this as the representation okay want to get p which is this vector we just input the zero position so it's 00 01 this is the zero position of f and will provide us this zero position seen by I which is this P. And if we would like to do the same for this axis which we'll assume this axis is the Xaxis then we will introduce it here.

[00:05:58]Uh this is just 1 0 0 0. Okay. And this will provide V. Okay. Which is is this okay? So this thing will go to here and then B will project onto the basis of the frame I. Okay, this is something uh you should know and just for the sake of making sure that we are on the same PL page. I will I I just explained this um same for the very basics of all the very basics of transformations. TI means you are doing nothing. how I sees itself. How does a frame sees itself?

[00:06:38]Well, this is the identity matrix.

[00:06:41]Um the inverse has um always exists and has a closed form. This closed form you should already know for rotations is the transpose and then for the translation is the negated of the of the rotation transpose plus the translation. Okay.

[00:07:00]Then in regards to inverse the inverse is switching off frames. Okay. So if you had the transformation as to how I is K and you want to invert this basically transformation that signals how K is I.

[00:07:16]Okay. And there should be the chaining.

[00:07:19]Okay. TI K composed by T K M is T I N. Okay. This is tied of what is a group because a group has a group operator which actually is here is the matrix multiplication in the case of the 4x4 matrix multiplication and basically also has an inverse. So this should exist as a neutral element.

[00:07:44]Okay. Top of that is manifold etc. But I will not delve into mathematics. This is just a review of the very basics you should be comfortable with in order to understand this tutorial.

[00:07:59]Okay. So let's del directly into the pose interpolation. As I said, we have an initial pose, a final pose. This is the input. The output in this case we will assume we only want two intermediate poses but we could choose just to have one or have three or have five whatever we want. Okay. This case there will be two.

[00:08:22]Okay. So what do we do here? Well we do what um something you should already know how how to do is the first thing we will do is remove the world frame. But because what we want is how the frame zero. So the initial frame sees the frame three. Okay. So if we have the this transformation of 0 to three which is this transformation the one that moves from here to here those interpolated transformations will be partial transformation not the complete thing but just a chunk of it.

[00:09:07]Okay, a piece of it. This would be 1/3 and this will be 2/3 of this large transformation. Okay, so we need to know t03.

[00:09:18]Okay, so what is t03? This you should know but this is uh this quite obvious is transformation from zero to world in and then composed by the transformation of world to three which is the last one.

[00:09:34]Okay, this we have here. Okay, and this we don't but this is the switching these indices. So we invert it. Okay, so this is how 0 63. Okay, so we have t 03. This is the whole transformation from here to here. So we need to take a chunk as I said, right? So how do we take a chunk?

[00:10:01]So this is the this is the idea. we we would like to take one/ird of it or 2/3 of it but we cannot okay in this transformation we cannot take a 4x4 representation projected representation of SC3 um and just divide it by three because this what we would do is go outside the manifold. So we no longer have an orthonormal matrix. The the um the elements are just um totally corrupted.

[00:10:34]Okay.

[00:10:36]But the idea is this. Okay. So I put between codes just to signal that this is the idea. Although mathematically we cannot do that. Okay.

[00:10:48]So how do we do it? So we do it by using Le algebra.

[00:10:54]So what are le algebbras? Well, le algebbras we will label in using this thing. So transformations which are elements of the SC3 group will be capital T and the algebbras we will call them B. Okay. Okay. So for the transformation from 0 to 3 will be v 03 and we will apply the logarithm this this function the logarithm to move this transformation to the le algebra. Also note that we have this weird symbol we will call it v because has the shape of a v that I will not explain. I might explain it in a later uh video but you should um assume that whenever you have a transformation and you apply logarithm you will also apply this V. Okay. And then you will have this vector. What is this vector? Well this vector is something that falls under R six and has three linear velocities and three angular velocities.

[00:12:05]Okay. And note one thing that is quite odd is that we are doing um a mapping and this will be actually one to one mapping because then the reverse we will reverse this with the exponential that we'll explain later on. So it's a this one to one mapping is mixing things right because this r basically comes from rotating angles. So comes from angles. You can use angles or you can use other representations but at the end of the day are three angles. Okay. So you have three angles that represent r. But here we are using radians/s. We are not using angles. We are using angular velocities.

[00:12:53]The same for the transformation. Here this is a transformation in meters. But here you cannot see meters. You see me m/ second. So this is linear velocities.

[00:13:06]Okay. So this is mixing apples and oranges and we need to understand how those are tied together because if you don't understand how they are related then you cannot interpret the le algebbras and you cannot interpret the le groups and how they are connected together. Okay.

[00:13:29]Okay. For though for that we I will tell a story. So so in the beginning at time equals 0 seconds we are the identity.

[00:13:40]Okay. So this is what happens in the beginning then we move at constant velocity. Okay.

[00:13:48]So we will move only at constant velocity and we will use the le algebra this v 03. So the output of the logarithm is the constant velocity at which we will move and for how long? 1 second. We will move 1 second at the le algebra constant velocity. And if we do that this identity will be converted or transformed into t03.

[00:14:17]Okay. So this is the idea. In the beginning we are the identity. We move at constant velocity in using the le algebra. We do that for just a second and we end up in the transformation that is the input of the logarithm.

[00:14:36]Okay. This is how they are related to each other.

[00:14:41]What is the advantage of the le algebra?

[00:14:44]Advantage of the le algebra is that now we can do these divisions.

[00:14:51]You remember before we have these double codes here because uh this was the idea but we couldn't do it mathematically.

[00:14:58]Now we can do it mathematically. We can divide it by three and we can multiply by 2/3 and here would be like multiply by 1/3. Okay, this is a vector space and remember a vector space is closed under the scalar multiplication. Okay, so this is valid and this is correct.

[00:15:18]So what do we do to get the transformations that I said there will be the inverse of the logarithm which will be the exponential. Okay. So the exponential you place this v 01 and then this uh sort of Mexican hat which is the inverse of the v and by doing that you get the transformation. Okay. So we are reversing the previous the process of the logarithm but now we have v 01 and v 02. So now we have these transformations. Okay, we recover the transformations.

[00:15:58]So as now we have these transformations, we can represent them in a world frame.

[00:16:06]So how the worlds is one it's just this transformation which came in the in the input as a representation of the initial frame multiplied by or composed if we think more in group theory by trans by t01 and the same for the second frame.

[00:16:28]Okay, good. So let's now delve a bit deeper into the le algebra. As I said this is a bit of an odd thing that uh we are using velocities here linear angular velocities and here we use radians and meters. So I explained this story that we start at time zero with the identity we move at constant speed using this le algebra and we end up here. Good. So let's try to del deeper and try to understand them better. For that we will dissect the le algebra. So we will start with the angular velocities.

[00:17:11]Okay the angular velocities are the easiest to understand.

[00:17:16]So with those angular velocities are related only to the rotation matrix or in the representation when you have translation and quaterian that would be the quat quaterian. Okay. But translation plays no role here. This is important. Okay. So, and what does this mean? Well, this says it's radians per second. But let's think about it. We are rotating using this constant angular velocity for a second. So, what does this mean?

[00:17:50]This is a almost primary school problem.

[00:17:53]Okay. So if I rotate at this angular velocity during 1 second obviously this is what I will rotate. This is the total angles I will rotate. So despite this be radians per second in the more purest form this would be also radians. Okay just because we do that in a constant velocity for a second. So there you go.

[00:18:20]Um so these are the radians of rotation around wx, wy and wz. Okay.

[00:18:28]And then note one thing. If we would have the axis of rotation and we would want to know how many radians it rotates around the axis of rotation. we can just take the norm of this chunk of the le algebra and we'll provide it as to to us. Okay. And then obviously the axis of rotation is nothing but this vector normalized. So you normalize this vector you get the axis of rotation you compute the length of this vector and you know the radians around this axis of rotation. Have you analyze each component of this vector and you know around each axis the radians it rotates?

[00:19:20]Okay. And you would say well this is like ural angles. No this is not uler angles and it's very important. U angles are all entangle one into the other. Okay. So it's a chain of rotations.

[00:19:39]those axis are totally independent.

[00:19:43]Okay? So you don't have gimbal lock.

[00:19:45]There is no gimbal lock here. Those axes are independent. It's necessary to be independent because if they were not independent, they would not form a vector space. So this is a vector space.

[00:19:58]This means that each one of those is independent and you don't have gimbal lock. It's very intuitive. In the user interface, you can draw this as as an axis in 3D.

[00:20:11]And this is very formative, very nice and sort of closely related to quat turning. So you want to transform this into quatnonian, it's pretty easy.

[00:20:23]Okay. So let's go now with the linear velocities which are the things that are a bit strange uh when you start using uh the logarithm in your league groups library and you try to understand what it generates and and it's not intuitive.

[00:20:44]Okay. And this is related to what the group does. Okay. So this is not um not a failure of the layout groups. It's just uh signaling how it behaves. Okay.

[00:20:56]So linear velocities. Okay. So what it depends the linear velocities not only depending on this translation it's also the rotation. Okay.

[00:21:07]Rotation will will drive also this. Okay. Um often times well this will be equal to this if the rotation is the identity. Okay. only in that case. Well, there could be other but this is the the main case. Okay, so the linear velocities depend on R and this translation. And how comes this happens? Well, this happens because the S3 group which is the main group we are analyzing is the semi-product between um the SO3 which is translation sorry rotation and T3 which is the translation. Okay, what does this uh semiroduct mean? Well, the semi-product means something very easily and common sensical. So say you have a robotic arm and the robotic arm the end defector is in a position and in an orientation right. So if you translate the base of the robotic arm you can translate uh the position of the endector but not the angle. The angle you won't be able to the orientation of the orientation of the the end defector you will not be able to change it. Okay. that if you rotate the base of of the of the robotic arm or if you move any axis of the robotic arm that will change the the angle. Chances are not always but it can happen that the translation and the orientation changes. So basically you made a translation and you change translation to change the orientation. Whenever you um move an axis in our body, chances are you do both. What does it mean? This means that when you change something in the rotation space, you may not only change the rotation but also translation. Okay? And this is the the idea intuitive idea of the semi-product.

[00:23:22]Okay. Let's put an example. Let's say that we have this in SC2. So this simple group into DX, VY and WZ. Okay. So 1 0 and minus P / 2 as this angular velocity. So we said that the angular um this angular velocity will be actually the radians we will rotate. So we will rotate clockwise 90°. So we start here at t = to0 and then after 1 second we should be here. Okay. And let's let's analyze this this linear velocity. So we move towards this uh axis and we rotate this direction.

[00:24:08]Okay. So if we move forward here and we rotate we will end up here. Okay. So the translation is 064 this and minus 064 this okay and this thing is different from this thing. Okay, just because we have this rotation that distorts the translation.

[00:24:34]Okay, this is important. And let's analyze the transformation.

[00:24:38]Transformation as I said depends on an angle in this case is one because it's into D. And this is the translation.

[00:24:45]Okay. And this you can observe that it's different from one zero. Okay. This is important because if you have a um a transformation and you apply the logarithm and then you analyze the linear velocities and the linear velocities are different uh from your translation. This shouldn't shock you. Okay, this is normal.

[00:25:12]And then we go for the silicase. So the silicase is the silicase is we don't have angular velocity so it's zero.

[00:25:21]We only moving x at 1 m/s. So we end up in at 1 meter.

[00:25:28]And in this case, yes, it's correct.

[00:25:30]Right? The translation 1 0 which is is is in this matrix is the same as the velocities. Why? Because this wz is zero. The angular velocity is zero.

[00:25:42]Okay.

[00:25:43]This is important as well.

[00:25:48]Okay. So let's review a bit more the exponential map. So you have this x, you add this linear algebra, you add the Mexican hat and then you get the transformation. Okay.

[00:26:08]Then what is the reverse? So the logarithm of this will provide this which is the same as this. Okay. So this is kind of a circular thing. Okay, you do the logarithm and then you can revert with the exponential and vice versa.

[00:26:24]Okay, important thing the exponential of the zero. This you will see a lot especially in machine learning because you are interested in zero vectors in the algebra very often because this means you reconstruct um transformation. So uh you will see zero when for example autoometry maxes a loop closure.

[00:26:47]Okay. So what does it mean zero mean zero is that you did nothing? You didn't move and you didn't spin. So you did nothing. And what does it mean that you did nothing? The identity. Okay. It was generated identity matrix because the identity matrix means you didn't.

[00:27:04]And this is the com compact form that all the good implementations of uh le groups and le algebbras must uh must obey. Okay, this is a very good thing you should add into a unit test. You can sample the algebbras, exponentiate them and check if they generate the transformation that we're generating from it. So, so basically um what you can do as well is take a URDF or robotic arm uniform not not necessarily un uniformly generate the algebra but uniformly generate uh thetas of the different angles of the axis of the robotic arm. This will give you transformation. Then you do the logarithm of this transformation and then the exponential of this transformation and you should recover t.

[00:27:59]Okay. So log the urdf uniformly sample thetas then the transformation do the logarithm and then the exponential and then the question do I regenerate the transformation? If it's yes all the time then it's a good implementation otherwise there is a back in your implementation. Okay.

[00:28:22]then nothing let's say that we want to utilize the groups the algebraas and this is tied to the example that I showed of the exponential of the zero okay so let's say that we want to optimize beta and lambda okay this can be intrig some um refinement of whatever you want of the sensors it doesn't matter beta and lambda I want to map this map f to fit this tar. Okay, so we have a data set on beta and lambda and t star which is our target transformation and we want to map this f this f let's say it's a neuron network.

[00:29:06]So this neural network will take those guys and try to fit this. Okay, if we wanted to do does that with our gravel algebra the problem is this needs to generate a transformation.

[00:29:17]Transformation have some restrictions.

[00:29:20]We don't want restrictions because they are very complicated. So we want something simple and easy.

[00:29:26]Okay. So what do we do? Well, we can make the f to generate le algebbras because if this generates the algebra, we are not constrained. We can generate anything in R6. So we can just make it spit out um six units, okay, without any activation function.

[00:29:48]And that's pretty much it. It will it will work right because the only thing you need to do is to add the exponential. The exponential in pipos and other good libraries will preserve gradients.

[00:30:01]The this Mexican hat preserves gradients and it's quite kind of easy and simple actually.

[00:30:09]And then you compose it with the inverse of t star which is your target. Okay. So what should you expect? Well, if f does great, this should be the identity, right? Because if this does perfect the this will be tar. So tstar inverse composed with t star is the is the is the identity, right? And if it does sort of not okay, it will be not the identity but not very far from the identity. And I will now explain what do I mean with not very far from. Okay. But let's finish this thing.

[00:30:52]Okay.

[00:30:53]So here we get the the transformation which in the optical case the optimal case will be zero and then we do the logarithm. Okay. Why? Because if we utilize this as a loss function right we need a scalar right the scaler what will be? well will be a number and the norm of the logarithm will give you the rate right scaler to back propagate your gradients in on manifold optimization right cuz this will be for example uh zero in the case that f does perfect and then the zero vector the norm of the zero vector will be the scalar zero so nothing will be back propagated right and note this and this is very important This the the concept that is very important is that the norm of the lee algebra is the length of the geodessic on the manifold.

[00:31:56]And what does this nonsense mean? That means that what I said if it does sort of okayish this should provide a larger number than zero. If it does bad, it should provide a number that is larger and it this does dreadful this should provide a very very very large number. Okay. So the idea is that this um le algebbras the le algebbras and the length of the le algebbras are a very good indicator as to the distance of this transformation that is inside right this is a very important uh property for the practical case you might wait it because you might be more interested in the angular error and the translation error And sometimes translation errors will be very large in kilometers. And if you measure them in in meters will be very large and and basically you will ignore the angle. So you have to most of the times you will weight it. In um say small robots like the UR5 like universal robot you may not wait it and you may apply directly this. Okay otherwise you would wait it.

[00:33:22]Good. So this is kind of um introduction to on manifold um optimization and how you would apply the algebbras in your for example PyTorch code to optimize neural networks.

[00:33:40]Okay. So now that we are well equipped and we know better let's do thing wrong.

[00:33:48]um let's do things wrong and for that we will basically do the interpolation but we make a mistake in the the the post interpolation and we will do that on purpose um to learn more about the algebra to learn more about the groups and also to review all the things that we um we already explained right because it's for a tool to be used to be used properly for you to to know how to use a tool you you should know when to use it and you should know when not to use it.

[00:34:22]Okay, so we covered the when to use it but we didn't cover the when not to use it. Okay, so now it's time to do that.

[00:34:30]Okay, so we have the initial frame, the final frame and the intermediates that we want as up. Okay, as I said previously, um if you remember, we wanted the T03 and then we make chunks of T03 by converting to the logarithm and then multiply by 1/3 and 2/3. Okay.

[00:34:51]But um you are used to using uh laplas transforms and fer transforms. And what you usually do is you transform into the the laplas domain or the fera domain. You do mathematics there and then you do the inverse laplas transform and the inverse fa transform right. And you think okay the same dynamic might apply here. I can do I can first do this apply the logarithm work in the logarithm because it's very easy very straightforward because it's a vector space. So what could be easier than that and then we do the exponential when we get the result and we are done. So we basically the logarithm transform into easy maths then we do the the computation on easy maths and then we do the exponential and we are done. um this will not work right but let's let's let's try to to see how it doesn't work okay so first we do this we transform everything into the algebbras okay so in this case what we want to do is transform into easy maths and then we will work with those uh le algebbras okay so vw0 and vw3 from tww0 and tww3 okay so we We don't remove the world frame, we keep it.

[00:36:18]Okay. So then what do we do? Well, we want the V 03 because we need to do chunks, right? But it's easy because basically the V03 is just the subtraction of those. Okay, I put here a question mark because this is a very good question mark, right? Because this will be not necessarily true. So in some cases it's true in some cases is not true. I will already advance for commutative groups it is true for non-commutatives it's not true. Okay but um this is the mistake we will assume we will make okay cuz we will assume this is true in all the cases. Okay even in the case that is not true it can be an approximation.

[00:37:05]Okay, but for now we're just going to assume this is potentially true. Okay, so what do we do? Well, we have V 03 and V 03.

[00:37:15]Uh basically from here we can construct V 01 and V 02. Okay, and we can do even this more generic like V 0 I and we multiply by I and we divide by three.

[00:37:29]Okay. And then we take this and we move into the exponential and we get the transformation from zero to i. Okay. As I said this this should not work. Okay.

[00:37:42]And the mistake was here. Okay. So what's going on here?

[00:37:49]Okay. So let's analyze what we did previously when it worked and what we are doing now that doesn't work. Okay, so the problem stems from the V 03, right? This is the problematic guy. So the V 03 is the logarithm of the transformation of T03.

[00:38:11]Okay, this is correct. There is no problem. And let's wait a second. What is T3? Well, well, T3 is TWW0 inverse composed by TWW3. Okay, this how zero says three. Okay, nice. But um we know a lot about um about the groups and the algebra now and let's go and let's delve deeper and let's decompose those. Okay, what is the exponential of those? Okay.

[00:38:46]So this is E of the minus because let's remember there is an inverse here. V W0 composed by E of VW3. Okay. So far this is totally correct. Okay. See we just did composite and this was what we used to get the right and correct solution.

[00:39:11]But now with our new approach, what we did is assume that this is actually this. And what is this? Well, this abstraction is just taking those exponents and moving them down. Okay, so you can see an X here. X means basically it's not equal. But why is not equal?

[00:39:36]How comes is not equal? What is the signal that here should tell us that this doesn't make any sense? Even if you don't know um if you don't know uh leg groups only algebra, you should kind of perceive that something is really off here. Why? Okay, so we are saying that this is equal to this sort to speak, right? Because it's the the the sort of internal. So the there is a mapping a one to one mapping and we're saying that this is not the same but this is equivalent this is equivalent to this and this is equivalent to this there is an equivalence okay there is a bjection of the elements and this is a bjection of the operations and what doesn't make any sense remember the here there is an operation which is the matrix multiplication which is the group composition And this composition is non-commutative.

[00:40:40]You cannot take this guy and move it here. Okay? Or take this guy and move it here is the same. You cannot do that.

[00:40:49]Right? Those don't commute.

[00:40:53]This operation that is here doesn't commute. And what is this arithmetic that we are doing here? Because this is a minus and there is a plus here. Right?

[00:41:01]Well, those commute because it's a subtraction. And I can move this here and it's valid. And I can move this here behind and it's valid. It is correct. So we are saying that there is um there is a onetoone representation of a non-commutative operation and a commutative operation and this doesn't make any sense. Okay. And those three guys who studied this very well uh gives us um how this works. Okay, usually this is called BCH. Okay, so basically in a compact form the logarithm of e to the x and e to the y is x + y + 12 of x y and those brackets. This is basically the commutator plus here is a chaining of commutators. So commutators and commutators this chain this goes into like a convoluted chain of um intertwined let's say like that is intertwining the commutations. Okay.

[00:42:15]And this one should sh sound you familiar because this looks a lot like what how you would do a tailor expansion by linearizing a nonlinear function.

[00:42:25]Okay. Or any function. Good. So anyways, what does this mean? This means that if you want to do this logarithm, you have a linear approximation that is x + y.

[00:42:42]Okay, but the whole thing has a lot of nonlinearities. This was the nonlinearities of the manifold groups. Okay, and the nonlinearities are driven by how it commutes. Okay. So let's say that we would use the group T2 or T3 which is the trans translations. This would be actually correct because translation don't commute. Okay?

[00:43:08]Just if you don't do any rotation, you just translate. You don't commute. It's a purely it's a pure fully commutative um commutative um group. So this commutator will give us uh zero because basically this commutator says how much x and y non-commute. Okay. So if they totally commute this is a zero and this is correct. Right? And this is actually how you would do uh interpolation.

[00:43:37]When you do interpolation, say in primary or secondary school, if you do some things silly in Python that you want to interpolate one point with another point, you do this, right? So you have just one line. So this silly interpolation obviously you can do this subtraction.

[00:43:55]The final position minus the initial position. then you divide by the steps and then you stack them up and you form a line. Okay. But in general when you have um interesting um groups like uh the SC3 that we are dealing with which is non-commutative this is not true.

[00:44:17]Okay. But nonetheless, this is actually also an approximation, right? And we will see later on that like you would do with a lot of robotics that you get actually the the Jacobian and things along those lines that you get linear approximations and you iterate on those linear approximations and this works out to do something that is nonlinear. But for here for this post interpolation, we don't do that. we do the correct way to do it. Okay, which we can do it which is using this right we don't do this this we don't do okay nice so now we are in the next problem which is the pose averaging so what is pose averaging post averaging basically you are at t minus one so the previous time step and in the previous time step you have an estimate of the transformation seen by the world frame TWW to T minus one.

[00:45:26]Okay. Then you utilize the sensors that you have in your robot to try to estimate where you are at time t which is where you are. So the inertial sensors will provide you this frame. So from t minus one to o will give you this transformation. You have very expensive 3D camera will give us will give us how we move from t minus one to also t. So that will be the c for the camera and then a very vgd or depth anything correcting for scaling that is important um will provide us this uh transformation and this frame. Okay. Now that we utilized all the sensors, what we need to do or one thing we could do potentially if the frames are different, chances are they are different is doing the average because we don't know if this is the right one or this or this. Okay, so we will average them out and I will do the simple average. We can do potentially weighted average but I will not explain it. Although in the code I did implement weight weighted average and this average will be this frame here. Okay. And this frame here is from t minus one to t.

[00:46:47]Okay. And we'll assume that we are exactly here. Okay. So basically the pose averaging can be used for example to detect how we move given the sensors we are providing. Okay good. So how we resolve this? Let's go step by step.

[00:47:09]First we take the algebbras of these transformations. Okay. From t minus one to this inertial sensor from t minus one to c the camera t minus one to a which will be the ai and we take the leat. So we start with the lever to be noted that it is those are small trans small transformations. We are assuming that here we are sampling at a decent frequency rate and we don't move hell a lot. We move a tiny bit. Okay. So those are the le algebbras.

[00:47:45]Good.

[00:47:48]So what do we do then? Well we are in the le algebra world. So in the le algebra world we can do arithmetic because we are in the vector space.

[00:47:59]Okay, so we can just add them up and divide them by three. Okay, and this will give us B tus one D star. Okay, there is a star and the star is literally the star of this lesson. Okay, so why did I say T star and not T?

[00:48:20]Okay, this is extremely important.

[00:48:23]Well, it is not T because of what we said. Um we said before right we said before that um what we cannot do is precisely do this and we are here doing precisely this.

[00:48:43]Okay. So what we are doing is saying that the average of those in the le algebra space is the same as the average uh if we would use the proper transformations. Okay. And this is just factually not true. But it is not exactly totally not true. Is it it is an approximation and is it is a linear approximation. Okay. So so long we are not very far off and we are in more or less a reasonable range we can assume that we are not going to get very far up from t.

[00:49:38]Okay. So that is why we use t star because here we are do we are we know we are not in t but in t star but we know t star will be not very far off because we just took a linear approximation. Okay good.

[00:50:00]Now we do the exponential and we take the transformation. And what do we do with the transformation?

[00:50:06]Well, what we do with the transformation is basically try to get new transformations with respect to this new frame. And why do we do that? Well, we are converging. Okay, so we are doing an iterative algorithm where this t star is closer to t. So we are closer to the solution. So let's start here. So this is our new starting point is t star which is very close to very close to t and we are going to do exactly the same but now we are much closer to t. What does it mean that if we do the linear approximation it will work even better because it's less nonlinear because we are closer we are less far off.

[00:50:58]Okay. So we do the same we take the algebbras and we do the average and this let's let's notice from t star to t star. So t star will be close to t and t star star star will be very very very close to t.

[00:51:17]Okay.

[00:51:18]And we say that we stop there. Okay.

[00:51:21]Because we already convert. So how does this look like? Okay. So we have this blue thing which was the pose at time t minus one. Then at as at as candidates for for t we have this for inertial this for the 3D camera and this for the AI.

[00:51:40]Okay. What is the first step? Well we get this le algebra this algebbras we do the average the exponential and we get t star. T star still will not be T because we know by BCH that what we did is not proper but as we know that those transformations are not very far off.

[00:52:06]Well we will make some mistake some error but not much. Okay so we are getting a bit close to T and then we start from here. So we get this transformation, this transformation and this transformation.

[00:52:23]We do the logarithm, we do the averaging and we get this transformation. So from t star to t star and t star it's already very very very close. Okay. So we can assume we stop here and we assume t star is a good approximation of t. Okay. So we go step by step. Okay. This is done in robotics all the time. And you have EKF and other algorithm where you all the time say okay I linearize here I iterate and by iterating it works and um graph slam works the same way. So this is no no surprise here. Okay so just to be more accurate here basically we will see that that this transformation from t minus one to t is approximate t minus one to t star. Note that we did two iteration but you can do as many as you want and in actually in actuality in the code you can choose the iterations you make. Okay, usually you will stop when you see that you more or less already converge. Okay, and what is this thing? Well, is the first iteration composed by the second iteration. Okay, because those collapse and you get t minus one and t star. Okay, and that's how you solve the problem. Okay, note that here there are even more um mathematical details that um that I didn't mention. For example, um the Nessie3 Nessie 3 I think it is called left invariant is not by variant.

[00:54:05]So you I think you left multiply by um some Jacobian to get better convergence etc etc which you don't necessarily need to do for rotations because rotations are by variant but those are a bit more complicated things and it's enough complication for that. Okay. Also bear in mind that sometimes you will not have the t minus one right. So you will have three poses and you need to average them out. So if you don't have t minus one and you have those three, what do you do?

[00:54:41]Right? So one thing you can do to average them out is say average the translations, do the logarithm of the of the rotations, uh average them out, create um create a transformation of those and this is a good initial seat. Okay, a good initial seat. S simply when you have those and you need to do averaging you try to get a good frame that is close enough so that when you run the iterative algorithm you get there quickly. Okay. Don't get the you can get the identity and you use the identity but if if those are very large especially in translation um it it it may do ill effect. So it's better to to do some quick dirty approximation so that you get some reference frame that is not very far and then you do this uh this transformation from this frame that is not very far to those.

[00:55:54]Okay. And if you do that, then you know that these linear approximations are very likely to converge as you iterate.

[00:56:03]Okay. Just uh watch out with this.

[00:56:07]And then the last thing that can be a bit confusing is um say that you are given a transformation from t minus one to t which is what we computed in the previous one and you are asked to know what are the velocities. Okay. And this is very common sensical probably you already know but uh just to make sure that you are you don't get confused by this tutorial. Obviously you will use the logarithm because the logarithm gives you velocities and this velocity is the velocity of the robot. Yes or no?

[00:56:38]Obviously this is not the velocity of the robot right cuz this le algebra comes from this story that you move from the identity to to this in 1 second but you don't move in one second you move in delta t and this is the implementation your your robot will have its own delta t which of your choice okay so basically the the real velocity is the logarithm divided by the delta t that you app.

[00:57:08]Okay.

[00:57:10]Uh well, that's it. Um there might be other things I could comment on but I think it is um it is enough for