Calculate transform from different poses

Question

My robot is at a starting pose $s_0 = (x_0, y_0, \theta_0)$ and, while moving, I receive a sequence of motion measurements $[\mu_1, \mu_2, \: ... \: \mu_n]$, where $\mu_i = (\Delta x_i, \Delta y_i, \Delta \theta_i)$, generating the path $[s_0, s_1, s_2, \: ... \: s_n]$.

At the first pose $s_0$ a landmark $l$ is measured with measurement $z_0 = (l_x, l_y)$, where $l_x$ and $l_y$ are the $x$ and $y$ coordinates of the landmark in the reference frame of $s_0$.

Here is an explanatory schema:

What I need is to find the landmark measurement $z_0$ in the reference frame of the last pose, $s_n$. In the image above, it would mean computing $z_3$, i.e. the measurement $z_0$ in the reference frame of $s_3$.

My idea is to use homogeneous coordinates to get the matrix transformations between poses, i.e.

\begin{equation} s_n = M_{n-1}^n M_{n-2}^{n-1}\: ... \: M_0^1 s_0 \end{equation}

and do the backward chaining of inverses to find $z_3$. But I don't really know how these matrices should be, and how to do the second step using the inverses.

Answer 1

I am just going to explain from the basics. So feel free to skip through the first part and scroll to the bottom if you want the answer.

Basics:

The 3 parameters of your pose are $x,y,\theta$.

These can be stored as homogeneous matrix which is the combination of the translation($x,y$) and the rotation($\theta$).

It looks like so

$$\begin{bmatrix} cos(\theta) & -sin(\theta) & x\\ sin(\theta) & cos(\theta) & y \\ 0 & 0 & 1 \end{bmatrix} $$.

The advantage of this homogeneous representation is that it allows us to chain together poses using matrix multiplication, and that the inverse pose/transform is equal to the matrix inverse.

Note when working with homogeneous points you may notice that the sizes of your landmarks(2x1 vector) and the transform(3x3) matrix don't match. So you also have to convert your landmarks to the homogeneous form by appending a 1 to the end. $[x,y] -> [x,y,1.0]$.

So each of your poses($M$). Can be represented by one of these 3x3 matrices.

So you will have $M_{s_1}^{w},...M_{sn}^{w}$

Your landmark position I'll represent as $l_{s0}$ or $l_{w}$ where the subscript is in which coordinate frame it is observed.

Your delta transforms can also be represented as a matrix. $$u_1 = \begin{bmatrix} cos(\delta\theta) & -sin(\delta\theta) & \delta x\\ sin(\delta\theta) & cos(\delta\theta) & \delta y \\ 0 & 0 & 1 \end{bmatrix} = M_{s_1}^{s_0} $$.

So now you can transform one pose into another by just multiplying it by your delta transform.

$$ M_{s_0}^{w} * u_1 = M_{s_0}^{w} * M_{s_1}^{s_0} = M_{s_1}^{w}$$

In this case I use $w$ to represent some arbitrary global frame in which all your poses and landmarks exist. For our purposes we can just say it is the origin $x=0,y=0,\theta=0$, and we can sometimes ignore writing it down($M_{s_1}^{w}=M_{s_1}$). I ,however, think it is good practice to always write it as it makes sure you chain your poses together correctly.

Multiplying transforms and landmarks

When you are multiplying landmarks and transforms your are essentially changing the coordinate frames.

$$ M_{s0}^{w} * l_{s0} = l_{w}$$

Here we are transforming a landmark in the $s0$ frame to the world frame$w$.

$$ M_{s0}^{w} * M_{s1}^{s0} = M_{s1}^{w}$$

Here we are computing the pose of the $s1$ frame in world. Note that we can also remove the intermediate coordinate frames because they essentially cancel each other out. Here $s0$ is cancelled.

You can also rearrange the equation (they are just matrices) if you want to compute something else.

$$ M_{s0}^{w} = M_{s1}^{w}* {M_{s1}^{s0}}^{-1}$$

The two key things to remember are that it is matrix multiplication(so order matters), and that coordinate frames should always match.

So for landmarks the subscripts must match.

OK $$ M_{s0}^{w} * l_{s0} = l_{w}$$

NOT OK $$ M_{s1}^{w} * l_{s0} = l_{w}$$

And for poses the subscript of the preceding term and the superscript of the after term must match.

OK $$ M_{s0}^{w} * M_{s1}^{s0} $$

NOT OK $$ M_{s0}^{w} * M_{s0}^{s1} $$

This rule even works with inverse because the inverse operation flips the subscript and superscript.

$${M_{s1}^{s0}}^{-1} = M_{s0}^{s1}$$

Solution:

Given:

• Landmark($l_{s0}$) in $s0$
• Pose $s0$ which is $M_{s_0}^{w}$ (I noticed afterwards you don't even need this information, since everything is relative for you.)
• delta transforms $u1,...,u3 = M_{s_1}^{s_0},...,M_{s_3}^{s_2}$

Want:

• Landmark in $s3$. ($l_s3$)

From the subscripts we know we want an equation that looks like the following.

$$l_{s3}=M_{s0}^{s3} * l_{s0} $$

So we want to find $M_{s0}^{s3}$.

So just chain together the transforms making sure that our coordinate frames match.

$$M_{s0}^{s3}=M_{s_2}^{s_3}M_{s_1}^{s_2}M_{s_0}^{s_1}$$

Remember that our inverse flips the subscript and superscript. So to get $M_{s_2}^{s_3}$ we just invert our delta transform ${M_{s_3}^{s_2}}^{-1}$.

Answer 1: $$l_{s3}=M_{s_2}^{s_3}M_{s_1}^{s_2}M_{s_0}^{s_1} * l_{s0} $$

Now this is actually kind of ugly because we have to invert all of our delta transforms. Another better option is to only invert the final transform.

$$M_{s0}^{s3}={M_{s3}^{s0}}^{-1}={(M_{s_1}^{s_0}M_{s_2}^{s_1}M_{s_3}^{s_2})}^{-1}$$

Answer 2: $$l_{s3}={(M_{s_1}^{s_0}M_{s_2}^{s_1}M_{s_3}^{s_2})}^{-1} * l_{s0} $$