Solve for transform parameters given original and transformed vectors

Question

I have some transformation in 3D homogeneous coordinates that includes three-axis rotation, translation and strain (linear deformation):

$$\\ T P = P' \\ T = D R_z R_y R_x S $$

$$ D = \begin{bmatrix} 1 & 0 & 0 & d_x \\ 0 & 1 & 0 & d_y \\ 0 & 0 & 1 & d_z \\ 0 & 0 & 0 & 1 \end{bmatrix}\\ R_z = \begin{bmatrix} \cos a & -\sin a & 0 & 0 \\ \sin a & \cos a & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}\\ R_y = \begin{bmatrix}\cos b & 0 & -\sin b & 0 \\ 0 & 1 & 0 & 0 \\ -\sin b & 0 & \cos b & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ R_x = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos c & -\sin c & 0 \\ 0 & \sin c & \cos c & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ S = \begin{bmatrix} m_x & 0 & 0 & 0 \\ 0 & m_y & 0 & 0 \\ 0 & 0 & m_z & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$

$P$ and $P'$ are identically-sized $4 \times n$, where $n$ is the number of points. I have some control over the number of points, but it should probably be at least 3.

Given $P$ and $P'$, I need to solve for all of the transformation parameters: $a, b, c, d_x, d_y, d_z, m_x, m_y, m_z$. If $n$ is set to 4, then at least $T = P'/P$ is a straightforward matrix inversion, though $n$ might end up being higher and I would probably have to use a pseudoinverse.

I will be using a numerical library (not yet chosen), but for the time being:

• How much of this solution can be done analytically?
• Is there a way of re-forming the problem to make the solution easier? (I considered spherical coordinates but I'm not very sure how I would go about that)

I tried a couple of brute-force numerical multivariate nonlinear solvers on this and they both (unsurprisingly) choked.

Answer 1

@dovalojd's answer is pretty good. There are better methods, though, of determining the nautical angles. You can use a test vector to eliminate the roll, $\theta_x$:

$ \hat n = Q\hat x \\ = R_z R_y R_x \hat x \\ = R_z R_y \hat x \\ = R_z \begin{bmatrix} \cos \theta_y \\ 0 \\ -\sin \theta_y \end{bmatrix} \\ \hat n = \begin{bmatrix} \cos \theta_y \cos \theta_z \\ \cos \theta_y \sin \theta_z \\ -\sin \theta_y \end{bmatrix} $

With the roll gone, you can focus on the other two. First, you find the yaw, $\theta_z$, using yaw = atan2(nx, ny). Then you calculate the pitch, $\theta_y$, using s = hypot(nx, ny); pitch = atan2(s, -nz). Though asin could be used directly on ny since $\theta_y \in [-\frac \tau 4,-\frac \tau 4]$, it is numerically unstable around $\pm \frac \tau 4$, so it should be avoided. atan2 doesn't suffer this problem.

To get the roll now, undo the yaw and pitch. First, the yaw. Recover $\cos \theta_z$ and $\sin \theta_z$ directly from $\hat n$:

$ \cos \theta_z = \frac {n_x} s \\ \sin \theta_z = \frac {n_y} s $

Use these to reconstruct $R_z$, then apply $R_z^T$ to $Q$'s left side, leaving $R_y R_x$. Do the same thing with $R_y$, leaving you with just

$R_x = \begin{bmatrix} 1 & & \\ & \cos \theta_x & -\sin \theta_x \\ & \sin \theta_x & \cos \theta_x \end{bmatrix} $

The roll can then be determined with one last atan2: roll = atan2(Rx_yy, Rx_yz). Again atan2 is preferred for stability reasons, but also to automatically cover the $\theta_x \notin [-\tau/4, \tau/4]$ case. For slightly better accuracy, you can repeat the roll calculation with the other two non-trivial elements of $R_x$ and do an average.

So, that's all three nautical angles! Though this answers the original question, I feel obligated to add that analysis using these angles suffers from Gimbal lock, so if you can do your analysis using Q, maybe you should. That or quaternions, spinors, Clifford algebras, or some other kind of posh upper class mathemagicks.

Answer 2

Assuming P is well conditioned, you can start with the pseudo-inverse, and compute $T=P'/P$. This should have a form approximately $T=\begin{bmatrix} a_{1,1}&a_{1,2}&a_{1,3}&d_x\\a_{2,1}&a_{2,2}&a_{2,3}&d_y\\a_{3,1}&a_{3,2}&a_{3,3}&d_z\\0&0&0&1\end{bmatrix}$. To compute an exact analytical answer, the bottom row of T must be exactly $\begin{matrix}0&0&0&1\end{matrix}$, but if there are only small errors, it may still be possible to calculate an estimated solution that is "good enough" for your purposes using analytical methods.

The values of $d_x$, $d_y$, and $d_z$, are clearly visible in T, so use these to compute $$R_zR_yR_xS=D^{-1}T=D^{-1}P'/P$$.

Next, let $Q=R_zR_yR_x$. Then $$(QS)^TQS=(D^{-1}P'/P)^T(D^{-1}P'/P)$$ $$(QS)^TQS=S^TQ^TQS$$ Note that $Q$ is a rotation matrix, so $Q^TQ=I$, which yields $$(QS)^TQS=S^TIS=S^TS=(D^{-1}P'/P)^T(D^{-1}P'/P)$$ S is a diagonal matrix, so its diagonal elements will equal the square root of the diagonal elements of $(D^{-1}P'/P)^T(D^{-1}P'/P)$.

Now it is possible to compute $Q=D^{-1}TS^{-1}=D^{-1}(P'/P)S^{-1}$. Here Wikipedia provides the results of the matrix multiplication, $$Q=R_zR_yR_x=\begin{bmatrix} cos(b)cos(c)&cos(a)sin(c)+sin(a)sin(b)cos(c)&sin(a)sin(c)-cos(a)sin(b)cos(c)&0\\-cos(b)sin(c)&cos(a)cos(c)+sin(a)sin(b)sin(c)&sin(a)cos(c)-cos(a)sin(b)sin(c)&0\\sin(b)&-sin(a)cos(b)&cos(a)cos(b)&0\\0&0&0&1\end{bmatrix}$$

Using this array, $b=arcsin(q_{3,1})$, $a=arccos(q_{3,3}/cos(b))$, and $c=arccos(q_{1,1}/cos(b))$.

If you resort to numerical methods, you may also find it useful (depending on the solver) to note that $det(D)=det(R_x)=det(R_y)=det(R_z)=1$ implies $det(S)=det(P'/P)$, and use this as an additional constraint.