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