I am stuck with a time-series problem of estimating 6DOF. I will use a simple example to explain what I need. Suppose there are 4 points about an origin o in 3D space i.e. $A$, $B$, $C$, $D$.
I will use $T_{ab}$ to denote transformation of $A$ to $B$ and so on.
$T_{ab}$ is a matrix in $\mathbb{R}^{4\times 4}$ of rotation + translation.
- Say I start at $A$ (which is the init point and all successive transformation is with respect to $A$) and move to $B$, hence I can get the transformation $T_{ab}$.
- Then I move from $B$ to $C$, but my algorithm will provide the transformation $T_{ac}$
Therefore: $T_{bc}$ = $T_{ac}$ * $(T_{ab})^{-1}$ ........ (eq.1)
The problem is if I update or reset my algorithm at $C$ it will move back to $A$. So the transformation using eq. 1 will be incorrect once the algorithm updates itself. That is if I move from $C$ to $D$, the transformation will be now begin from $A$ instead of $C$ since the update resets the init point (I cant change this behavior as its important for some other purposes).
What I have done: I kept track (multiple) of all intermediate transformations given my equation 1 above but it seems not be be working.
What is one way I can keep the transformation correct mathematically even after the program resets.
To back out a transformation, you apply the inverses of the transformations in reverse order.
So if you have three transformations, each consisting of a rotation and a translation:
$$T_{3t}T_{3r}T_{2t}T_{2r}T_{1t}T_{1r}\vec{v} = \vec{v}_t$$
Then to get $\vec{v}$ again you do this:
$$T_{1r}^{-1}T_{1t}^{-1}T_{2r}^{-1}T_{2t}^{-1}T_{3r}^{-1}T_{3t}^{-1}\vec{v}_t = \vec{v}.$$
I think it's just a matter of keeping track of how you transform the vector and then just unrolling it to get back to the start.