Dados 3 puntos de un cuerpo rígido en el espacio, ¿cómo encuentro la orientación correspondiente (también conocida como rotación o actitud)?

Question

Digamos que mido las posiciones 3D, $\mathbf{p_1(t), p_2(t), p_3(t)} \in \mathbb{R}^3$ de tres puntos en el espacio que están conectados por un cuerpo rígido en el tiempo $t = t_0$ . Luego, hago una segunda medición, en $t = t_1$ después de que el cuerpo haya girado y trasladado. ¿Cómo puedo determinar la orientación correspondiente a ese movimiento?

O bien una matriz de rotación R ( $\in SO3$ ) o cuaternión q ( $\in H$ ) está bien. Me gustaría implementar esto en el software y estoy buscando una solución rápida, idealmente sin el uso de funciones de biblioteca de alto nivel (por ejemplo, Matlab qr() o oth()).

Supongo que queremos satisfacer las siguientes ecuaciones:

$$\mathbf{p_1}(t_1) = \mathbf{R}\ \mathbf{p_1}(t_0) + \mathbf{t} $$ $$\mathbf{p_2}(t_1) = \mathbf{R}\ \mathbf{p_2}(t_0) + \mathbf{t} $$ $$\mathbf{p_3}(t_1) = \mathbf{R}\ \mathbf{p_3}(t_0) + \mathbf{t}$$

Dónde $\mathbf{R}$ es la rotación que estoy buscando y $\mathbf{t}$ es la traducción.

Answer 1

He publicado esta respuesta a una pregunta similar en sci.math. A continuación transcribo la pregunta y el resumen de la solución. Para este problema, no necesitamos calcular $r$ , sólo hay que ponerlo en $1$ .

Regresión multilineal conformada por mínimos cuadrados

Dado $\{ P_j : 1 \le j \le m \}$ y $\{ Q_j : 1 \le j \le m \}$ dos conjuntos de puntos, queremos encontrar un mapa conforme, definido por un mapa lineal, $M$ , y un vector, $R$ que mapea un conjunto de puntos a otro a través de $$ Q = P M + R\tag{1} $$ donde requerimos que $M M^T = r^2 I$ y que el residuo cuadrado $$ \sum_{j=1}^m\left|P_jM+R-Q_j\right|^2\tag{2} $$ es mínima.

Resumen del método

Para encontrar la solución por mínimos cuadrados de $P M + R = Q$ para un conjunto determinado de $\{ P_j \}$ y $\{ Q_j \}$ bajo la restricción de que el mapa sea conforme, primero calculamos los centroides $$ \overline{P}=\frac1m\sum_{j=1}^mP_j\qquad\text{and}\qquad \overline{Q}=\frac1m\sum_{j=1}^mQ_j $$ A continuación, calcula la matriz $$ \begin{align} S &=\sum_{j=1}^m\left(Q_j-\overline{Q}\right)^T\left(P_j-\overline{P}\right)\\ &=\sum_{j=1}^mQ_j^TP_j-m\overline{Q}^T\overline{P} \end{align} $$ Sea la descomposición del valor singular de $S$ sea $$ S=UDV^T $$ Próximo cálculo $\{ c_k \}$ con $$ \begin{align} c_k &=\sum_{j=1}^m\left[\left(P_j-\overline{P}\right)V\right]_k\left[\left(Q_j-\overline{Q}\right)U\right]_k\\ &=\sum_{j=1}^m\left[P_jV\right]_k\left[Q_jU\right]_k-m\left[\overline{P}V\right]_k\left[\overline{Q}U\right]_k \end{align} $$ y definir $$ a_k = \mathrm{sgn}( c_k ) $$ Dejemos que $I_k$ sea la matriz con el $(k,k)$ elemento establecido como $1$ y todos los otros elementos ajustados a $0$ . A continuación, calcule $$ E=\sum_{k=1}^na_kI_k $$ Calcular la matriz ortogonal $$ W=VEU^T $$ Si $\det(W) < 0$ pero $\det(W) > 0$ es necesario, cambiar el signo del $a_k$ asociado a la $c_k$ con el menor valor absoluto.

Si es necesario, calcule $r$ por $$ r\sum_{j=1}^m\left|P_j-\overline{P}\right|^2=\sum_{j=1}^m\left\langle\left(P_j-\overline{P}\right)W,Q_j-\overline{Q}\right\rangle $$ o de forma equivalente $$ r\left(\sum_{j=1}^m\left|P_j\right|^2-m\left|\overline{P}\right|^2\right) =\sum_{j=1}^m\left\langle P_jW,Q_j\right\rangle-m\left\langle\overline{P}W,\overline{Q}\right\rangle $$ Finalmente, tenemos el mapa conforme deseado $Q = P M + R$ donde $$ M = r W $$ y $$ R = \overline{Q} - \overline{P} M $$ Más información, más fácil de calcular

Supongamos que se quiere mapear $\{P_i\}_{i=1}^3$ a $\{Q_i\}_{i=1}^3$ y las distancias entre los $P_i$ y $Q_i$ son iguales. Calcula un cuarto punto mediante $$ P_4=P_1+(P_2-P_1)\times(P_3-P_1) $$ y $$ Q_4=Q_1+(Q_2-Q_1)\times(Q_3-Q_1) $$ A continuación, cree la matriz $P$ cuyas columnas son $P_2-P_1$ , $P_3-P_1$ y $P_4-P_1$ .

Crea también la matriz $Q$ cuyas columnas son $Q_2-Q_1$ , $Q_3-Q_1$ y $Q_4-Q_1$ .

Entonces $x\mapsto QP^{-1}x+(Q_1-QP^{-1}P_1)$ asigna los puntos de origen a los puntos de destino.

Answer 2

Puedes resolverlo así: 1. Defina un marco de referencia base que no se mueva, por ejemplo $F_0$ . Esto podría ser simplemente su marco de referencia para estas 3 posiciones que tiene ( $p_i$ ) 2. Definir el marco de referencia, por ejemplo $F_r$ unido al cuerpo rígido mediante 3 puntos ( $p_i$ ). Por ejemplo, con $p_0$ como origen, vector unitario en $p_1$ de $p_0$ como eje x y en $p_2$ de $p_0$ como eje y y eligiendo el eje z por la regla de la mano derecha. 3. Suponiendo que T es una matriz de transformación homogénea, se conoce la orientación de los puntos en ambos marcos de referencia $F_0$ y $F_r$ y también el desplazamiento de $F_r$ En $F_0$ por lo que se podría resolver T como un problema inverso. Sin embargo, todavía se necesita la función de inversión de la matriz.