Eficiente y precisa aplicación numérica de la fórmula de rotación inversa Rodrigues (matriz de la rotación-> ángulo de eje)

Question

Quiero implementar la Inversa Rodrigues Rotación de la Fórmula (también conocido como Registro de mapa de SO(3) a so(3)), en doble precisión (código de MATLAB está muy bien para el ejemplo), preferiblemente como un 3-vector de parámetros con el vector de dirección de la unidad de ajuste de escala de la magnitud de la rotación.

En la analítica de la forma es (de Wikipedia):

$\theta = \arccos\left( \frac{\mathrm{trace}(R) - 1}{2} \right)$

y, a continuación, utilizarlo para encontrar la normalizado eje:

$\omega = \frac{1}{2 \sin(\theta)} \begin{bmatrix} R(3,2)-R(2,3) \\ R(1,3)-R(3,1) \\ R(2,1)-R(1,2) \end{bmatrix}$

que luego puede ser utilizado para encontrar la escala del eje de rotación $\rho = \theta \omega$

---

Por supuesto, $\sin(\theta)$ hará que el denominador acercarse a cero, que es indefinido. La rotación de vectores $\rho$ a cero rotación es $\rho = \begin{bmatrix}0 & 0 & 0\end{bmatrix}^T$. Además, también será indefinido en $n\pi$, aunque podemos asignar la rotación a ser la señal deseada ($\pm\pi$)

El ingenuo aplicación es en caso de que() instrucciones de punto flotante de cerca de los valores de $n\pi$ rotaciones, pero no hay duda de que es una forma mejor que algunos sucia hacks en torno a las singularidades... ¿verdad?

---

EDITAR:

En las rotaciones de cerca de cero, empíricamente, las siguientes obras:

if (trace(R) > (3 - small_number))

    inverse_sinc = 1 + (1.0 / 6.0)      * theta_2 + ...
                       (7.0 / 360.0)    * theta_4 +
                       (31.0 / 15120.0) * theta_6;

    rho = 0.5 * inverse_sinc * r;

end

donde $\theta$ (y los poderes de los mismos), y $\mathbf{r} = \begin{bmatrix} R(3,2)-R(2,3) \\ R(1,3)-R(3,1) \\ R(2,1)-R(1,2) \end{bmatrix}$ son pre-calculados, y inverse_sinc (es decir, x/sen(x))se calcula a partir de la serie de Taylor. Este es exacta al mejor de 10^-11 en cada eje cuando la unidad probada a través de un rango de valores (0, eps, 10^-12 a través de 10^-3 y valores negativos para cada uno a través de los tres ejes).

Una buena solución para $\theta = \pi$ todavía se me escapa...

Answer 1

Deje $\mathbf{R}\in SO(3)$ ser una matriz de rotación, $t=R_{1,1} + R_{2,2} + R_{3,3}$ ser la huella de $\mathbf{R}$, e $\mathbf{r}=\begin{bmatrix} R(3,2)-R(2,3) \\ R(1,3)-R(3,1) \\ R(2,1)-R(1,2) \end{bmatrix}$.

Podemos calcular la rotación de vectores $\omega$ (eje angular de la representación) de la siguiente manera:

$$\omega = \begin{cases} \left(\frac{1}{2} - \frac{t-3}{12}\right)\mathbf{r} & \text{if}\quad t\ge3-\epsilon\\ \frac{\theta}{2\sin(\theta)}\mathbf{r} & \text{if}\quad 3-\epsilon > t > -1+\epsilon\\ \pi\frac{\mathbf{v}}{|\mathbf{v}|} & \text{if }\quad t\le -1+\epsilon \end{casos} $$ con $$\theta = \arccos\left( \frac{t - 1}{2} \right)$$ y $(w,\mathbf{v})$ ser una unidad de cuaterniones $$ v_a = \frac{s}{2},\quad v_b = \frac{1}{2}(R_{b,a}+R_{a,b}),\quad v_c = \frac{1}{2}(R_{c,a}+R_{a,c})\\ \quad\text{con} \quad s := \sqrt{R_{a,a}-R_{b,b}-R_{c,c} + 1}\\ \text{y}\quad a := \underset{i\in\{1,2,3\}}{\arg\max}\{R_{i,i}\},\quad b := (a+1)\text{ mod } 3, \quad c := (a+2)\text{ mod }3~.$$

---

Antecedentes: El último caso de $\theta\approx \pm \pi$ ( $t\approx-1$ ) se calcula mediante la ruta: matriz de rotación $\Rightarrow$ unidad de cuaterniones $\Rightarrow$ eje angular.* Aquí, $\pi$ es el límite de $2\arctan\left(\frac{|\mathbf{v}|}{w}\right)$ con $w = \frac{1}{2s}(R_{c,b}-R_{b,c})$.

(* matriz de rotación de la unidad de cuaterniones de referencia: Nombre de la biblioteca que de nuevo se refiere a Ken Shoemake, "Cuaterniones Cálculo y Rápido de Animación", de 1987; unidad de cuaterniones para eje-ángulo de referencia: C. Herzberg et al.: "La integración de Sensor Genérico Algoritmos de Fusión con el Estado del Sonido La representación a través de la Encapsulación de los Colectores" de Información de la Fusión, 2011)

---

Edit: sería bueno tener un mayor orden de aproximación para la $t\le-1+\epsilon$ de los casos. Por favor, deja un comentario o editar si usted tiene una buena solución...