Compute path in absolute frame from sequence of measurements

Question

I have a sequence of measurements $\{(x_i, y_i, \theta_i)\}_{i = 1 \:...\: N}$. Each of these represents the new pose of the robot seen from the previous pose, i.e. a measurement $(x', y', \theta')$ means that the robot moved of $x'$ and $y'$ units in the $x$ and $y$ axis of the previous coordinates system and rotated of an angle $\theta'$ afterward. Therefore, the value of $\theta$ represents the rotation of the new coordinates system w.r.t. the previous.

How can I compute the absolute path (the sequence of all poses expressed in the absolute frame)?

Answer 1

Assuming frame $0$ is the 'absolute frame', if we let $^j P_i$ be the $i^{th}$ position/orientation expressed in the $j^{th}$ coordinate frame, then what you're asking for is the sequence $$\{(^0P_i)_{i=1 ... N}\},$$ correct?

Using your sequence of measurements $\{(x_i, y_i, \theta_i)_{i=1 ... N}\}$, it's straightforward to compute the $i^{th}$ 2-D transformation matrix,

$$^i_{i+1} T = \begin{pmatrix} \cos \theta_i & -\sin \theta_i & x_i\\ \sin \theta_i & \cos \theta_i & y_i \\ 0 & 0 & 1 \end{pmatrix}$$

And since you have all of them, use the fact that $$^0_i T = ^0_1T ^1_2 T ... ^{i-1}_i T,$$ then just iterate through to list to compute each translation matrix and compute the data you're interested in. Note that you have to append a $1$ to each $P_i$ to make the 3x3 multiplcations work out (as noted in the reference):

$$^0P_{i+1} = ^0_i T ^i P_{i+1}$$

Hopefully that's helpful. Here's a video explaining frame transformations, that I often refer back to. It's really easy to get lost in notation.

EDIT: To compute the orientation of the $i^{th}$ coordinate, you can use the rotational component of the $^0_iT$ translation matrix, since you have the cos and sin of the angle in entries (1,1) and (2,1), just use the atan2 function to pull out $\theta$. Make sense?

Answer 2

Each step can be represented by its transformation matrix, $$ \begin{bmatrix} \cos{\theta'_{i}} & -\sin{\theta'_{i}} & x'_{i}\\ \sin{\theta'_{i}} & \phantom{-}\cos{\theta'_{i}} & y'_{i} \\ 0 & 0 & 1 \end{bmatrix}. $$ The world position at each step $i$ is the rightward-propagating product of the transforms up to that point, $$ \begin{bmatrix} \cos{\theta_{i}} & -\sin{\theta_{i}} & x_{i}\\ \sin{\theta_{i}} & \phantom{-}\cos{\theta_{i}} & y_{i} \\ 0 & 0 & 1 \end{bmatrix} = \prod_{1}^{i} \begin{bmatrix} \cos{\theta'_{i}} & -\sin{\theta'_{i}} & x'_{i}\\ \sin{\theta'_{i}} & \phantom{-}\cos{\theta'_{i}} & y'_{i} \\ 0 & 0 & 1 \end{bmatrix}. $$

(This is the same math as is used for the forward kinematics of a robot arm -- each step is a link with geometry $x',y'$ followed by a joint rotation $\theta'$.)