# Calculating relationship between two coordinate frames with known relationships between each coordinate frame and a third one

Suppose I have three coordinate frames, W, D and C.

I will express all translation vectors as $T$ and rotation matrices as $R$, where $T ∈ \Bbb R^3$ and $R ∈ SO(3)$.

I know the translation and rotation from the origin of W to the origin of D, which I denote $T_d^w$ and $R_d^w$ respectively. I also know the translation and rotation from the origin of C to the origin of D, which I denote $T_d^c$ and $R_d^c$ respectively.

$$T_d^w =\begin{bmatrix} 0.1 & 0.1 & 0.1 \\ \end{bmatrix}$$ $$T_d^c =\begin{bmatrix} 0 & 0 & 0.2 \\ \end{bmatrix}$$

$$R_d^w =\begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}$$

$$R_d^c =\begin{bmatrix} 1 & 0 & 0 \\ 0 & -1 & 0 \\ 0 & 0 & -1 \\ \end{bmatrix}$$

$R_d^w$ is identity, ie. no rotation, and $R_d^c$ is a rotation of 180 degrees about the x axis.

I am trying to work out $T_c^w$ and $R_c^w$.

I believe rotations can be multiplied through to get to the final rotation, ie:

$R_c^w = R_d^w R_c^d$

I can get $R_c^d$ by taking the inverse of $R_d^c$, and because of orthogonality, I can just take the transpose ie. $R_c^d = (R_d^c)^T$

$R_c^w = R_d^w (R_d^c)^T$

Now for the translation, I believe the following can be used:

$T_c^w = T_d^w + R_d^w T_c^d$

Since we don't have $T_c^d$, but we do have $T_d^c$, we can get it from the rotation:

$T_c^d = R_d^c T_d^c$

EDIT: This assumption is incorrect. See answer.

which can then be substituted above, resulting in the following final equation for Tw_c:

$T_c^w = T_d^w + R_d^w (R_d^c T_d^c)$

When I work through this, my final translation is: $$T_c^w =\begin{bmatrix} 0.1 & 0.1 & -0.1 \\ \end{bmatrix}$$

However I was expecting

$$\begin{bmatrix} 0.1 & 0.1 & 0.3 \\ \end{bmatrix}$$

What am I doing wrong? Or am I missing a step?

• Please use MathJax. Commented Nov 29, 2017 at 7:17
• @Sou燈馬想 edited to use mathjax Commented Nov 30, 2017 at 0:16

The error was the assumption:

$T_c^d = R_d^c T_d^c$

Which only takes into account rotation between the coordinate frames, and not the translation.

I used the fact that a point $p$, in C coordinate space can be related to D as follows:

$p^c = T_d^c + R_d^c p^d$

Rearranging, taking the inverse of the rotation matrix and using the orthogonality rule gives:

$p^d = (R_d^c)^T (p^c - T_d^c)$

Because we want the translation from origin of D to C, I use the fact that the point in C coordinate frame is the origin. $p^d$ then becomes $T_d^c$.

$T_c^d = (R_d^c)^T (\begin{bmatrix} 0 \\ 0 \\ 0 \\ \end{bmatrix} - T_d^c)$

So my final equation becomes:

$T_c^w = T_d^w + R_d^w ((R_d^c)^T (\begin{bmatrix} 0 \\ 0 \\ 0 \\ \end{bmatrix} - T_d^c))$

When I evaluate this, I get my result \begin{bmatrix} 0.1 & 0.1 & 0.3 \\ \end{bmatrix}

As expected.