# Compute path in absolute frame from sequence of measurements

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)?

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?

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'$$.)