Resolver los parámetros de transformación dados los vectores originales y transformados

Question

Tengo algunas transformaciones en coordenadas homogéneas 3D que incluyen rotación de tres ejes, traslación y deformación (deformación lineal):

$$\\ 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$ y $P'$ son de tamaño idéntico $4 \times n$, donde $n$ es el número de puntos. Tengo cierto control sobre el número de puntos, pero probablemente deberían ser al menos 3.

Dados $P$ y $P'$, necesito resolver todos los parámetros de la transformación: $a, b, c, d_x, d_y, d_z, m_x, m_y, m_z$. Si $n$ se establece en 4, entonces al menos $T = P'/P$ es una inversión de matriz directa, aunque $n$ podría ser mayor y probablemente tendría que usar una pseudoinversa.

Voy a utilizar una biblioteca numérica (aún no elegida), pero por el momento:

• ¿Cuánto de esta solución se puede hacer de manera analítica?
• ¿Hay alguna manera de reformular el problema para hacer la solución más fácil? (Consideré coordenadas esféricas pero no estoy muy seguro de cómo debería proceder)

Probé un par de solucionadores numéricos multivariables no lineales de fuerza bruta en esto y ambos (no sorprendentemente) se atascaron.

Answer 1

Suponiendo que P está bien condicionado, puedes comenzar con la pseudo-inversa y calcular $T=P'/P$. Esto debería tener una forma aproximada de $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}$. Para obtener una respuesta analítica exacta, la fila inferior de T debe ser exactamente $\begin{matrix}0&0&0&1\end{matrix}$, pero si existen solo pequeños errores, aún puede ser posible calcular una solución estimada que sea "suficientemente buena" para tus propósitos utilizando métodos analíticos.

Los valores de $d_x$, $d_y$, y $d_z, son claramente visibles en T, así que úsalos para calcular $$R_zR_yR_xS=D^{-1}T=D^{-1}P'/P$$.

Luego, deja $Q=R_zR_yR_x$. Entonces $$(QS)^TQS=(D^{-1}P'/P)^T(D^{-1}P'/P)$$ $$(QS)^TQS=S^TQ^TQS$$ Nota que $Q$ es una matriz de rotación, así que $Q^TQ=I$, lo cual nos da $$(QS)^TQS=S^TIS=S^TS=(D^{-1}P'/P)^T(D^{-1}P'/P)$$ S es una matriz diagonal, por lo tanto, sus elementos diagonales serán iguales a la raíz cuadrada de los elementos diagonales de $(D^{-1}P'/P)^T(D^{-1}P'/P)$.

Ahora es posible calcular $Q=D^{-1}TS^{-1}=D^{-1}(P'/P)S^{-1}$. Aquí Wikipedia proporciona los resultados de la multiplicación de matrices, $$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}$$

Usando este arreglo, $b=arcsin(q_{3,1})$, $a=arccos(q_{3,3}/cos(b))$, y $c=arccos(q_{1,1}/cos(b))$.

Si recurras a métodos numéricos, también puede resultar útil (dependiendo del solucionador) notar que $det(D)=det(R_x)=det(R_y)=det(R_z)=1$ implica que $det(S)=det(P'/P)$, y utilizar esto como una restricción adicional.

Answer 2

La respuesta de @dovalojd es bastante buena. Sin embargo, hay mejores métodos para determinar los ángulos náuticos. Puedes usar un vector de prueba para eliminar el balanceo, $\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} $

Con el balanceo eliminado, puedes centrarte en los otros dos. Primero, encuentras el acimut, $\theta_z$, usando yaw = atan2(nx, ny). Luego calculas el cabeceo, $\theta_y$, usando s = hypot(nx, ny); pitch = atan2(s, -nz). Aunque asin podría usarse directamente en ny ya que $\theta_y \in [-\frac \tau 4,-\frac \tau 4]$, es inestable numéricamente alrededor de $\pm \frac \tau 4$, por lo que se debe evitar. atan2 no sufre este problema.

Para obtener el balanceo ahora, deshaz el acimut y el cabeceo. Primero, el acimut. Recupera $\cos \theta_z$ y $\sin \theta_z$ directamente de $\hat n$:

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

Utiliza estos para reconstruir $R_z$, luego aplica $R_z^T$ al lado izquierdo de Q, dejando $R_y R_x$. Haz lo mismo con $R_y$, quedando solo

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

El balanceo entonces se puede determinar con un último atan2: roll = atan2(Rx_yy, Rx_yz). Nuevamente, atan2 es preferido por razones de estabilidad, pero también para cubrir automáticamente el caso de $\theta_x \notin [-\tau/4, \tau/4]$. Para una precisión ligeramente mejor, puedes repetir el cálculo del balanceo con los otros dos elementos no triviales de $R_x$ y promediar.

¡Así que esos son los tres ángulos náuticos! Aunque esto responde a la pregunta original, siento la obligación de agregar que el análisis utilizando estos ángulos sufre de Bloqueo de Cardán, así que si puedes hacer tu análisis utilizando Q, tal vez deberías hacerlo. Eso o cuaterniones, spinors, álgebras de Clifford, u otro tipo de matemagia de clase alta elegante.