# Find rigid body transform $T_b$, given transforms $T_a, T_c$, where $T_c = T_b^{-1}T_aT_b$ for $T_b$. i.e. find change of basis.

Let $$\mathbf{T}_a, \mathbf{T}_b, \mathbf{T}_c \in SE(3)$$ be rigid body transforms, and;

$$\begin{equation} \mathbf{T}_c = \mathbf{T}_b^{-1}\mathbf{T}_a\mathbf{T}_b. \label{eq_basis} \end{equation}$$

We wish to find $$\mathbf{T}_b$$ given $$\mathbf{T}_a, \mathbf{T}_c$$.

I think this is equivalent to finding a change of basis between $$\mathbf{T}_a$$ and $$\mathbf{T}_c$$. My approach so far has been to decompose the problem into rotational and translational constraints as follows.

Let $$\mathbf{R}_a \in SO(3)$$ be the rotational component of $$\mathbf{T}_a$$, and $$\mathbf{t}_a$$ be the translational component. Then we have:

$$\begin{bmatrix}\mathbf{R}_c & \mathbf{t}_c\\ \mathbf{0} & 1\end{bmatrix} = \begin{bmatrix}\mathbf{R}^{\top}_b & -\mathbf{R}^{\top}_b\mathbf{t}_b\\ \mathbf{0} & 1\end{bmatrix} \begin{bmatrix}\mathbf{R}_a & \mathbf{t}_a\\ \mathbf{0} & 1\end{bmatrix}\begin{bmatrix}\mathbf{R}_b & \mathbf{t}_b\\ \mathbf{0} & 1\end{bmatrix}$$

Expanding this equation and collecting terms gives the following set of matrix equations;

$$(1)\ \ \ \mathbf{R}_c = \mathbf{R}^{\top}_b\mathbf{R}_a\mathbf{R}_b\\ (2)\ \ \ \mathbf{t}_c = \mathbf{R}^{\top}_b\mathbf{R}_a\mathbf{t}_b + \mathbf{R}^{\top}_b\mathbf{t}_a - \mathbf{R}^{\top}_b\mathbf{t}_b$$

In general, (1) has infinitely many solutions which form a circle. We can find them by the following;

Let $$\mathbf{c}$$ be the rotation axis of $$\mathbf{R}_c$$, i.e. $$\mathbf{R}_c\mathbf{c} = \mathbf{c}$$. Then;

$$\mathbf{R}_c\mathbf{c} = \mathbf{R}^{\top}_b\mathbf{R}_a\mathbf{R}_b\mathbf{c} \\ \mathbf{c} = \mathbf{R}^{\top}_b\mathbf{R}_a\mathbf{R}_b\mathbf{c} \\ \mathbf{R}_b\mathbf{c} = \mathbf{R}_a\mathbf{R}_b\mathbf{c}$$

This means $$\mathbf{R}_b\mathbf{c}$$ is parallel to the rotation axis of $$\mathbf{R}_a$$. Hence $$\mathbf{R}_b$$ can be taken as any rotation taking $$\mathbf{a}$$ to $$\mathbf{c}$$, i.e. a rotation of the rotation axes.

As stated, there are infinitely many such rotations. We may choose $$\mathbf{R}_b$$ to be for example the rotation in the plane given by $$\mathbf{a}\times\mathbf{c}$$ by the angle $$acos(\frac{\mathbf{a}\cdot\mathbf{c}}{|\mathbf{a}||\mathbf{c}|})$$. There are a few degenerate cases, for example if $$\mathbf{R}_c = \mathbf{I}$$ or $$\mathbf{R}_a = \mathbf{I}$$, or if $$\mathbf{R}_c = \mathbf{R}_a$$. I dealt with those here.

This takes care of (1).

Substituting our choice for $$\mathbf{R}_b$$ into (2), and solving for $$\mathbf{t}_b$$ we find;

$$\left(\mathbf{R}_a - \mathbf{I}_{3\times3}\right)\mathbf{t}_b = \mathbf{R}_b\mathbf{t}_c - \mathbf{t}_a$$

Where $$\mathbf{I}_{3\times3}$$ is the 3x3 identity matrix. The term $$\mathbf{R}_a - \mathbf{I}_{3\times3}$$ gives a singular matrix since $$\mathbf{R}_a$$ has only 1 as its eigenvalue. The resulting system therefore has zero or infinitely many solutions for any choice of $$\mathbf{R}_b$$.

When does this system have solutions at all? What are the additional constraints? Is there a neater way of approaching this problem? What additional constraints are necessary to give a unique solution?