How to derive the camera trajectory from ICP

Question

This may be a very simple question so please bear with me...

Suppose I have a stationary object and point clouds, $n$ $(x,y,z)$ points, of that object taken by a moving camera at time steps $t_i$, $t_{i+1}$, ...

Using ICP on the point clouds from subsequent time steps I get the relative transformation (a rotation matrix and translation vector $\in SE3$) needed to map the cloud from some $t_i$ to to the cloud at $t_{i + 1}$.

Is the Camera trajectory from time $t_i$ to $t_{i + 1}$ the same? ie I simply take the position of the camera at $t_i$ and multiply it by the rotation matrix and then add the translation vector to get the new position?

Answer 1

Yes that is correct. Easiest way is probably to work with the homogeneous 4x4 Tranform Matrix($T$) composed of $\begin{bmatrix}R & t\\0 & 1\end{bmatrix}$. Then your new pose is then just $T_i$ multiplied by $\Delta T_{i+1}^{i}$(relative transform). For every new relative transform just do this concatenation.

So $T_{i+1}=T_i*\Delta T_{i+1}^{i}$

Note:

Depending on your ICP implementation you may actually get the inverse relative transform $\Delta T_{i}^{i+1}$. If this happens then just invert the matrix and continue as normal.