¿Cómo calcular la matriz de rotación a partir de otros valores conocidos?

Question

Estimada comunidad de Math Stackexchange, ¿cómo puedo calcular $R$ matriz de rotación en esta ecuación ( $P=Rp+t$ )? (Véase la figura. Debajo de la figura hay una explicación más detallada).

La ecuación es para la transformación de $p=p_1,p_2,p_3$ puntos. $p$ está en el marco de referencia Oxyz (local). $p$ consta de 3 puntos de $p_1,p_2,p_3$ . Y $p_1,p_2,p_3$ cada uno tiene 3 coordenadas de $x,y,z$ . Por lo tanto, p es una matriz de 3x3. $p=\begin{bmatrix} p_{1x} & p_{2x} & p_{3x} \\ p_{1y} & p_{2y} & p_{3y} \\ p_{1z} & p_{2z} & p_{3z} \end{bmatrix}$

$t$ es la distancia entre el marco de referencia Oxyz(local) y OXYZ(global). $t=\begin{bmatrix} t_x \\ t_y \\ t_z \end{bmatrix}$ . Para poder realizar operaciones matriciales, podemos tomar $t$ como $t=\begin{bmatrix} t_x&t_x&t_x \\ t_y&t_y&t_y \\ t_z&t_z&t_z \end{bmatrix}$

$P$ son las coordenadas de p_1, p_2 y p_3 en el marco de referencia global OXYZ. $P$ es una matriz de 3x3. $P=\begin{bmatrix} P_{1x} & P_{2x} & P_{3x} \\ P_{1y} & P_{2y} & P_{3y} \\ P_{1z} & P_{2z} & P_{3z} \end{bmatrix}$

$R$ es la matriz de rotación (ortogonal). $R=\begin{bmatrix} r_{11}&r_{12}&r_{13} \\ r_{21}&r_{22}&r_{23} \\ r_{31}&r_{32}&r_{33} \end{bmatrix}$

Por lo tanto, en general: $P=Rp+t$

$\begin{bmatrix} P_{1x} & P_{2x} & P_{3x} \\ P_{1y} & P_{2y} & P_{3y} \\ P_{1z} & P_{2z} & P_{3z} \end{bmatrix} = \begin{bmatrix} r_{11}&r_{12}&r_{13} \\ r_{21}&r_{22}&r_{23} \\ r_{31}&r_{32}&r_{33} \end{bmatrix}.\begin{bmatrix} p_{1x} & p_{2x} & p_{3x} \\ p_{1y} & p_{2y} & p_{3y} \\ p_{1z} & p_{2z} & p_{3z} \end{bmatrix}+\begin{bmatrix} t_x&t_x&t_x \\ t_y&t_y&t_y \\ t_z&t_z&t_z \end{bmatrix}$

Podemos derivar la siguiente forma de la ecuación de $P=Rp+t$

$(P-t)=R.p$

$(P-t).p^{-1}=R$

$p$ es una matriz arbitraria, y no siempre se puede tomar su inversa. No puedo tomar la inversa de $p$ si el último crudo de $p$ es cero.

Veamos un ejemplo para que quede claro: $P=Rp+t$

$p_1=(1,1,0)$ , $p_2=(2,4,0)$ , $p_3=(5,10,0)$ son las coordenadas de tres puntos. Así que en forma de matriz: $p=\begin{bmatrix} 1 & 2 & 5 \\ 1 & 4 & 10 \\ 0 & 0 & 0 \end{bmatrix}$

$t=\begin{bmatrix} 0 \\ 0 \\ 50 \end{bmatrix}$

$\alpha=10^\circ $ (rotación alrededor del eje x)

$\beta=15^\circ $ (rotación alrededor del eje y)

$\gamma=20^\circ $ (rotación alrededor del eje z)

$R_x=\begin{bmatrix} 1 & 0 & 0 \\ 0 & cos(\alpha) & -sin(\alpha) \\ 0 & sin(\alpha) & cos(\alpha) \end{bmatrix}$

$R_y=\begin{bmatrix} cos(\beta) & 0 & sin(\beta) \\ 0 & 1 & 0 \\ -sin(\beta) & 0 & cos(\beta) \end{bmatrix}$

$R_x=\begin{bmatrix} cos(\gamma) & -sin(\gamma) & 0 \\ sin(\gamma) & cos(\gamma) & 0 \\ 0 & 0 & 1 \end{bmatrix}$

$R=R_z.R_y.R_x$

Por lo tanto, para $\alpha=10^\circ $ , $\beta=15^\circ $ , $\gamma=20^\circ $ la matriz de rotación es :

$R=\begin{bmatrix} 0.9077 & -0.2946 & 0.2989 \\ 0.3304 & 0.9408 & -0.0760 \\ -0.2588 & 0.1677 & 0.9513 \end{bmatrix}$

$P=R.p+t$

$P=$$\begin {bmatrix} 0.9077 & -0.2946 & 0.2989 \\ 0.3304 & 0.9408 & -0.0760 \\ -0.2588 & 0.1677 & 0.9513 \end {bmatrix} $$.\begin{bmatrix} 1 & 2 & 5 \\ 1 & 4 & 10 \\ 0 & 0 & 0 \end{bmatrix}$ $+\begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 50 & 50 & 50 \end{bmatrix}$

$P=\begin{bmatrix} 0.6131 & 0.6370 & 1.5925 \\ 1.2712 & 4.4239 & 11.0597 \\ 49.9089 & 50.1533 & 50.3832 \end{bmatrix}$

$p_1=(0.6131,1.2712,49.90)$ , $p_2=(0.6370,4.4239,50.1533)$ , $p_3=(1.5925,11.0597,50.3832)$ .

Ahora tomemos el ejemplo opuesto con los mismos valores para encontrar $R$ con los valores de $P,p,t$ como se sabe.

$P=R.p+t$

$\begin{bmatrix} 0.6131 & 0.6370 & 1.5925 \\ 1.2712 & 4.4239 & 11.0597 \\ 49.9089 & 50.1533 & 50.3832 \end{bmatrix}=R.$ $\begin{bmatrix} 1 & 2 & 5 \\ 1 & 4 & 10 \\ 0 & 0 & 0 \end{bmatrix}+$ $\begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 50 & 50 & 50 \end{bmatrix}$

¿Podría encontrar $R$ aquí en la ecuación anterior?

Para repetir la formulación:

$P=Rp+t$

$(P-t)=Rp$

$(P-t).p^{-1}=R.p.p^{-1}$

$(P-t).p^{-1}=R$

Para la inversa de $p$ (donde $p$ es $\begin{bmatrix} 1 & 2 & 5 \\ 1 & 4 & 10 \\ 0 & 0 & 0 \end{bmatrix}$ ), no puedo tomar la inversa de $p$ .

Aquí estoy atascado. Es interesante para mí, porque para la condición de que $R,p,t$ es conocido, $P$ se puede calcular (9 elementos desconocidos en la matriz 3x3 de $P$ ). Sin embargo, para la condición de que $P,p,t$ es conocido, estoy teniendo problemas para calcular $R$ (9 elementos desconocidos en la matriz 3x3 de $R$ ). Debería ser capaz de calcular el equivalente $R$ para ese problema inverso.

¿Tiene alguna idea sobre cómo encontrar $R$ ¿matriz de rotación? Se agradecerá cualquier sugerencia.

Gracias de antemano.

Answer 1

En primer lugar, creo que la gimnasia extra con $t$ es una distracción, así que plantearé el problema como:

$\mathbf{Q}=\left[\mathbf{q}_1\,\dots,\mathbf{q}_k\right]=\mathbf{P}-\mathbf{t}=\mathbf{R}.\boldsymbol{\mathcal{P}}=\mathbf{R}.\left[\mathbf{p}_1\,\dots\,\mathbf{p}_k\right]$

Dónde $\mathbf{Q}$ es algo $3\times k$ matriz de valor real, que también podemos ver como $k$ vectores columna $\mathbf{q}_{1,\dots k}$ . $\mathbf{R}$ es una matriz de rotación de valor real, lo que significa que es ortogonal ( $\mathbf{R}^T\mathbf{R}=\mathbf{Id}=identity$ ) y tiene un determinante de $det\left[\mathbf{R}\right]=+1$ . Por último, la matriz $\boldsymbol{\mathcal{P}}$ se compone de 3 vectores columna de valor real $\mathbf{p}_{1,\dots k}$ . El objetivo es determinar la matriz de rotación.

Aquí, para simplificar, trabajaré con conjuntos linealmente independientes de $\mathbf{p}$ -vectores, por lo que $k=\mbox{rank}\left(\mathbf{Q}\right)=\mbox{rank}\left(\boldsymbol{\mathcal{P}}\right)$

$k=\mbox{rank}\boldsymbol{\mathcal{P}}=3$

Si puedes elegir tres vectores linealmente independientes $\mathbf{p}_{1,2,3}$ puede tener una solución única para $\mathbf{R}$ dado $\mathbf{Q}$ (invirtiendo $\boldsymbol{\mathcal{P}}$ ). Si no puede tal solución única no existe.

Utilice rank función para comprobar si tiene un $\mathbf{p}$ -vectores

$k=\mbox{rank}\boldsymbol{\mathcal{P}}=2$

Si sólo tienes dos vectores linealmente independientes, creo que lo mejor que puedes obtener en principio son dos posibles soluciones, de las cuales ambas se ajustan a los datos. En algunos casos especiales aún podrás obtener la solución única.

Utilice Gram-Schmidt proceso para extraer dos vectores ortonormales $\mathbf{\hat{n}}_{1,2}$ que puede expresarse como $\mathbf{\hat{n}}_{1,2}=\alpha_{1,2}\mathbf{p}_1+\beta_{1,2}\mathbf{p}_2$ .

Habrá otros dos vectores ortonormales $\mathbf{\hat{q}}_{1,2}=\alpha_{1,2}\mathbf{q}_1+\beta_{1,2}\mathbf{q}_2$ .

Ahora trata de encontrar $\psi$ que minimiza: $s=\left|\left(\cos\psi\,\mathbf{\hat{q}_1+\sin\psi\,\mathbf{\hat{q}}_2}\right)-\left(\cos\psi\,\mathbf{\hat{n}_1+\sin\psi\,\mathbf{\hat{n}}_2}\right)\right|$ esto le dará la proyección del eje alrededor del cual la matriz $\mathbf{R}$ provoca la rotación. Que el eje de rotación real sea $\boldsymbol{{\hat{\omega}}}$ y su proyección en el plano abarcado por $\mathbf{\hat{n}}_{1,2}$ sea $\boldsymbol{{\omega}}_\mathcal{P}$ . La lógica aquí es simple si $\boldsymbol{\hat{\omega}}=\left(\cos\psi\,\mathbf{\hat{n}_1+\sin\psi\,\mathbf{\hat{n}}_2}\right)$ entonces la rotación de dicho vector no lo cambiará en absoluto, por definición.

Dos casos interesantes son cuando $s=const$ para todos $\psi$ . Entonces el eje de su matriz de rotación es perpendicular al plano abarcado por $\mathbf{\hat{n}}_{1,2}$ y puede hacer que el problema sea 2d (es decir, hacer $\mathbf{R}$ 2d) y encontrar la solución exacta.

Otro caso interesante es cuando $s=0$ o muy cerca de 0, digamos 1e-3, para algunos $\psi$ . Entonces encontraste una forma de expresar el eje de rotación en términos de tus vectores ortonormales: $\boldsymbol{\hat{\omega}}=\left(\cos\psi\,\mathbf{\hat{n}_1+\sin\psi\,\mathbf{\hat{n}}_2}\right)$ . Creo que podrás calcular el ángulo de rotación de $\mathbf{R}$ en este caso observando lo que ocurre con $-\sin\psi\mathbf{\hat{n}}_1+\cos\psi\mathbf{\hat{n}}_2$ , es decir, el vector que es perpendicular al eje de rotación.

En general, si encuentra $\boldsymbol{\omega}_\mathcal{P}$ se puede extender a todo el eje de rotación añadiéndole una componente del vector perpendicular al plano abarcado por $\mathbf{\hat{n}}_{1,2}$ , $\mathbf{\hat{m}}=\mathbf{\hat{n}}_1\times \mathbf{\hat{n}}_2$ . Así que

$$ \boldsymbol{\hat{\omega}}=\frac{\boldsymbol{\omega}_{\mathcal{P}}+f\mathbf{\hat{n}}_1\times \mathbf{\hat{n}}_2}{\left|\boldsymbol{\omega}_{\mathcal{P}}+f\mathbf{\hat{n}}_1\times \mathbf{\hat{n}}_2\right|} $$

Habrá ambigüedad en la elección del signo de $f$ - pero esa es la no-unicidad para ti. Una vez que tengas el eje de rotación, deberías ser capaz de reducir el problema a 2d de nuevo, y resolverlo exactamente (hasta el signo de $f$ )

$k=\mbox{rank}\boldsymbol{\mathcal{P}}=1$

No estoy seguro de que haya mucho que puedas hacer aquí.

Answer 2

Las matrices pueden hacer mucho más que rotar. Creo que esto puede ser parte de lo que dificulta el problema. En efecto, para determinar completamente una transformación lineal general en $\mathbb R^3,$ que es lo que un real general $3\times3$ codificación de matrices, necesitas saber qué pasa con al menos tres vectores linealmente independientes. En este caso más general, su $p$ matriz debe ser invertible para que haya una solución única.

Limitar el alcance de la transformación a una rotación cambia el problema drásticamente. Se pasa de $9$ grados de libertad en una transformación lineal general en $\mathbb R^3$ (un grado de libertad para cada entrada en un $3\times3$ matriz) a sólo $3$ grados de libertad.

Debido a los limitados grados de libertad de una rotación, no se necesita tanta información. Basta con saber lo que ocurre con dos vectores que no son linealmente dependientes (es decir, no pueden ser colineales). (Es decir, no pueden ser colineales.) Incluso eso te dará mucha información redundante, porque ya sabemos que las longitudes de los vectores no cambiarán y el ángulo entre ellos no cambiará.

No estoy seguro de cómo explotar este conocimiento extra en un algoritmo que se basa puramente en la aritmética matricial. Pero seguro que hay formas de extraer la información necesaria de las dos matrices $$ \begin{bmatrix} P_{1x} & P_{2x} & P_{3x} \\ P_{1y} & P_{2y} & P_{3y} \\ P_{1z} & P_{2z} & P_{3z} \end{bmatrix} - \begin{bmatrix} t_x&t_x&t_x \\ t_y&t_y&t_y \\ t_z&t_z&t_z \end{bmatrix} \quad\text{and}\quad \begin{bmatrix} p_{1x} & p_{2x} & p_{3x} \\ p_{1y} & p_{2y} & p_{3y} \\ p_{1z} & p_{2z} & p_{3z} \end{bmatrix}.$$ De hecho, sólo deberías necesitar las dos primeras columnas de cada matriz. La respuesta a Cálculo de la rotación de un par de vectores unitarios dados los estados inicial y final utiliza los dos vectores "antes de la rotación" para generar una base ortogonal, y lo mismo con los dos vectores "después de la rotación", y calcula la rotación de una base a la otra (para lo cual el método de inversión de la matriz funcionará, ya que los tres vectores base son linealmente independientes). O puedes considerar la respuesta de G Cab, que comienza con un poco de aritmética vectorial tridimensional para establecer una solución matricial.

Answer 3

Por razones de claridad, vamos a utilizar mayúsculas para denotar Matrices, y minúsculas para los vectores, así que vamos a hablar de $$ {\bf q}_{\,k} = {\bf R}\,{\bf p}_{\,k} + {\bf t} $$ y $$ {\bf Q} = {\bf R}\,{\bf P} + {\bf T}\quad \left| {\;{\bf T} = \left( {\matrix{ {t_{\,x} } & {t_{\,x} } & {t_{\,x} } \cr {t_{\,y} } & {t_{\,y} } & {t_{\,y} } \cr {t_{\,z} } & {t_{\,z} } & {t_{\,z} } \cr } } \right)} \right. = \left( {\matrix{ {{\bf t}\;,} & {{\bf t}\;,} & {{\bf t}\;} \cr } } \right) $$

1. Una matriz de rotación es ortogonal, lo que significa que preserva el producto punto de dos vectores y el módulo de su producto cruzado.
   En consecuencia, los dos triángulos $P,Q$ deben ser congruentes, incluyendo la reflexión, ya que estamos tratando con una forma 2D incrustada en 3D. Si son congruentes entonces tendremos $$ \left\{ \matrix{ \left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) \cdot \left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) = \left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right) \cdot \left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right) \hfill \cr \left( {{\bf q}_{\,3} - {\bf q}_{\,1} } \right) \cdot \left( {{\bf q}_{\,3} - {\bf q}_{\,1} } \right) = \left( {{\bf p}_{\,3} - {\bf p}_{\,1} } \right) \cdot \left( {{\bf p}_{\,3} - {\bf p}_{\,1} } \right) \hfill \cr \left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) \cdot \left( {{\bf q}_{\,3} - {\bf q}_{\,1} } \right) = \left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right) \cdot \left( {{\bf p}_{\,3} - {\bf p}_{\,1} } \right) \hfill \cr \left| {\left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) \times \left( {{\bf q}_{\,3} - {\bf q}_{\,1} } \right)} \right| = \left| {\left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right) \times \left( {{\bf p}_{\,3} - {\bf p}_{\,1} } \right)} \right| \hfill \cr} \right. $$ donde las etiquetas se permutarán si no hay una correspondencia punto a punto entre $\bf p$ y $\bf q$ .

2. Un triángulo se encuentra en un plano. Si ese plano pasa por el origen, los vectores de posición de los vértices son coplanares, es decir, son dependientes, por lo que la matriz ( $\bf P$ y/o $\bf Q$ ) tiene determinante nulo y no es invertible.
   Además, un triángulo que está lejos del origen, al ser actuado por una matriz de rotación experimentará una rotación sobre uno de sus puntos más una traslación de ese punto.

3. Entonces el enfoque más práctico es construir las matrices $$ \eqalign{ & {\bf \Delta P} = \left( {\matrix{ {{\bf p}_{\,2} - {\bf p}_{\,1} \;,} & {{\bf p}_{\,3} - {\bf p}_{\,1} \;,} & {\left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right) \times \left( {{\bf p}_{\,3} - {\bf p}_{\,1} } \right)} \cr } } \right) \cr & {\bf \Delta Q} = \left( {\matrix{ {{\bf q}_{\,2} - {\bf q}_{\,1} \;,} & {{\bf q}_{\,3} - {\bf q}_{\,1} \;,} & {\left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) \times \left( {{\bf q}_{\,3} - {\bf q}_{\,1} } \right)} \cr } } \right) \cr} $$ que, por las identidades de congruencia establecidas anteriormente, dará $$ \overline {{\bf \Delta P}} \,{\bf \Delta P} = \overline {{\bf \Delta Q}} \,{\bf \Delta Q} $$ y $$ \left| {\,{\bf \Delta Q}\,} \right| = \left| {\,{\bf \Delta P}\,} \right| $$

4. Ahora tenemos $$ {\bf \Delta Q} = {\bf R}\,{\bf \Delta P} $$ y podemos recuperar $\bf R$ porque $\bf \Delta P$ es invertible (si el triángulo no está degenerado).
   Para determinar $\bf t$ tenemos entonces $$ \eqalign{ & \left( {{\bf q}_{\,2} - {\bf q}_{\,1} } \right) = {\bf R}\left( {{\bf p}_{\,2} - {\bf p}_{\,1} } \right)\quad \Rightarrow \cr & \Rightarrow \quad {\bf q}_{\,2} = {\bf R}\,{\bf p}_{\,2} + {\bf q}_{\,1} - {\bf R}\,{\bf p}_{\,1} \quad \Rightarrow \cr & \Rightarrow \quad {\bf t} = {\bf q}_{\,1} - {\bf R}\,{\bf p}_{\,1} = {\bf q}_{\,2} - {\bf R}\,{\bf p}_{\,2} = {\bf q}_{\,3} - {\bf R}\,{\bf p}_{\,3} \cr} $$

En conclusión y en palabras llanas

• traslada ambos triángulos para tener un vértice en el origen;
• añadir a cada triángulo el producto cruzado de las aristas desde el origen, para construir dos tetraedros;
• girar un tetraedro para que coincida con el otro;
• traduce de nuevo los triángulos.

Ejemplo

Tomando los datos de su ejemplo $$ \eqalign{ & {\bf P} = \left( {\matrix{ 1 & 2 & 5 \cr 1 & 4 & {10} \cr 0 & 0 & 0 \cr } } \right)\quad \Rightarrow \quad {\bf \Delta P} = \left( {\matrix{ 1 & 4 & 0 \cr 3 & 9 & 0 \cr 0 & 0 & { - 3} \cr } } \right)\quad \left| {\,{\bf \Delta P}\,} \right| = 9 \cr & {\bf Q} = \left( {\matrix{ {0.6131} & {0.6370} & {1.5925} \cr {1.2712} & {4.4239} & {11.0597} \cr {49.9089} & {50.15330} & {50.3832} \cr } } \right)\quad \Rightarrow \cr & \Rightarrow \quad {\bf \Delta Q} = \left( {\matrix{ {0.0239} & {0.9794} & { - 0.8970} \cr {3.1527} & {9.7885} & {0.2280} \cr {0.2444} & {0.4743} & { - 2.8538} \cr } } \right)\quad \left| {\,{\bf \Delta Q}\,} \right| = 9.0008 \cr} $$

Entonces $$ {\bf R} = {\bf \Delta Q\Delta P}^{\, - \,{\bf 1}} = \left( {\matrix{ {0.9077} & { - 0.2946} & {0.2990} \cr {0.3304} & {0.9408} & { - 0.0760} \cr { - 0.2589} & {0.1678} & {0.9513} \cr } } \right)\quad \left| {\,{\bf R}\,} \right| = 1.0001 $$ y $$ {\bf t} = {\bf q}_{\,1} - {\bf R}\,{\bf p}_{\,1} = \left( {\matrix{ 0 \cr 0 \cr {50} \cr } } \right) $$ y estos compruebe con $$ {\bf Q} = {\bf R}\,{\bf P} + {\bf T}\quad \quad {\bf R} = {\bf R}_{\,{\bf z}} (\gamma ){\bf R}_{\,{\bf y}} (\beta ){\bf R}_{\,{\bf x}} (\alpha ) $$

Answer 4

Una de las formas más sólidas de encontrar la matriz de rotación sería probablemente utilizando El problema de Wahba que en su caso tiene asociada la siguiente función de costes

$$ J(R) = \frac{1}{2} \sum_{k=1}^3 a_k \| P_k - t_k - R\,p_k\|^2, \tag{1} $$

con $a_k > 0$ pesos (pero si no tiene ninguna incertidumbre en $P_k,t_k,p_k\in\mathbb{R}^3$ se puede poner a uno). Esta función de coste tendrá un mínimo global con $R\in SO(3)$ si los vectores $P_k-t_k$ o $p_k$ abarcan al menos un plano (que sería el caso de su ejemplo).

El mínimo de la función de coste de $(1)$ se puede encontrar utilizando

$$ B = \sum_{k=1}^3 a_k (P_k - t_k)\,p_k^\top. \tag{2} $$

Ahora encuentra el descomposición del valor singular de $B$ tal que

$$ B = U\,S\,V^\top, \tag{3} $$

donde $U$ y $V$ son matrices unitarias y $S$ una matriz diagonal. La matriz de rotación $R$ que minimiza $(1)$ puede obtenerse utilizando

$$ R = U \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(U)\det(V) \end{bmatrix} V^\top. \tag{4} $$