Matriz jacobiana de la fórmula de Rodrigues (mapa exponencial)

Question

Estoy trabajando en un algoritmo que debe alinear un par de imágenes. El modelo de movimiento, que describe la pose $p$ de una imagen (con respecto a la segunda) en el espacio 3D, es puramente rotacional.

Teoría a mi pregunta:

Según la fórmula de Rodrigues para las matrices de rotación, la matriz exponencial $R(p)=e^{\hat{p}}$ para un vector $p=(p_x,p_y,p_z)^T\in\mathbb{R}^3$ viene dada por $$\begin{align} R(p)&=e^{\hat{p}}=I + \frac{\hat{p}}{||p||} \sin(||p||) + \frac{\hat{p}^2}{||p||^2} (1-\cos{\theta}) \end{align}$$ donde $||p||=\sqrt{p_x^2+p_y^2+p_z^2}$ y $$\begin{align} \hat{p} &= \begin{bmatrix} 0 & -p_z & p_y \\ p_z & 0 & -p_x \\ -p_y & p_x & 0 \end{bmatrix} \end{align}.$$

Resumiendo los elementos de $R(p)$ produce $$\begin{align} R(p) = \begin{bmatrix} 1-(p_y^2+p_z^2)s & -p_z r+p_xp_y s & p_y r+p_xp_z s \\ p_z r+p_xp_y s & 1-(p_x^2+p_z^2)s & -p_x r+p_yp_z s \\ -p_z r+p_xp_z s & p_x r+p_yp_z s & 1-(p_x^2+p_y^2)s \end{bmatrix} \end{align}$$ donde $r=\frac{\sin(||p||)}{||p||}$ y $s=\frac{(1-\cos(||p||))}{||p||^2}$ se introdujeron para mayor claridad.

Ahora, dejemos que $R^s(p)\in\mathbb{R}^{9\times 1}$ sea la versión apilada de $R(p)$ es decir, todas las columnas se apilan en una sola columna y deja que $R^s_i(p)$ denotan el $i$ -éste es el elemento de $R^s(p)$ donde $1\leq i \leq 9$ .

La matriz jacobiana $J_{R}\in\mathbb{R}^{9\times 3}$ con respecto al vector $p$ viene dada por $$\begin{align} J_{R}&=\frac{\partial R^s}{\partial p} \\\\ &= \begin{bmatrix} \frac{\partial R^s_1}{\partial p_x} & \frac{\partial R^s_1}{\partial p_y} & \frac{\partial R^s_1}{\partial p_z} \\\\ \vdots & \vdots & \vdots \\\\ \frac{\partial R^s_9}{\partial p_x} & \frac{\partial R^s_9}{\partial p_y} & \frac{\partial R^s_9}{\partial p_z} \end{bmatrix} \end{align}$$

El primer elemento de $J_R$ puede expresarse entonces como $$\begin{align} \frac{\partial R^s_1}{\partial p_x} &=\frac{\partial}{\partial p_x} \Bigl(1-(p_y^2+p_z^2)\frac{(1-\cos(||p||))}{||p||^2}\Bigr) \\\\ &=-\Bigl[(p_y^2+p_z^2)\frac{\sin(||p||) p_x ||p|| - (1-\cos(||p||) 2 p_x)}{||p||^4}\Bigr] \end{align}$$ el segundo como $$\begin{align} \frac{\partial R^s_1}{\partial p_y} &=\frac{\partial}{\partial p_y} \Bigl(1-(p_y^2+p_z^2)\frac{(1-\cos(||p||))}{||p||^2}\Bigr) \\\\ &=-\Bigl[2p_y\frac{1-\cos(||p||)}{||p||^2}+(p_y^2p_z^2)\frac{\sin(||p||) p_y ||p|| - (1-\cos(||p||) 2 p_y)}{||p||^4}\Bigr] \end{align}$$ y el tercer elemento de la primera fila como $$\begin{align} \frac{\partial R^s_1}{\partial p_z} &=\frac{\partial}{\partial p_z} \Bigl(1-(p_y^2+p_z^2)\frac{(1-\cos(||p||))}{||p||^2}\Bigr) \\\\ &=-\Bigl[2p_z\frac{1-\cos(||p||)}{||p||^2}+(p_y^2p_z^2)\frac{\sin(||p||) p_z ||p|| - (1-\cos(||p||) 2 p_z)}{||p||^4}\Bigr] \end{align}$$

Ya es un Muchas gracias por haber leído hasta aquí ;-)

Mi pregunta: ¿Es la derivación de estos 3 primeros elementos de $J_R$ ¿Matemáticamente correcto? Si he hecho algo mal, ¿podríais detectar el error o los errores y ayudarme a corregirlos?

Cuando lo tenga todo claro, publicaré la matriz jacobiana completa como respuesta a esta pregunta. Para que cualquiera que se enfrente al mismo problema pueda volver a comprobar sus propios resultados...

La razón por la que necesito este jacobiano:

La postura $p$ de una imagen en la aplicación mencionada anteriormente puede describirse mediante un eje de rotación unitario y un ángulo (en radianes). Alternativamente, el eje unitario puede ser escalado de acuerdo con el ángulo de rotación, que es lo que estoy utilizando. Para hacer el mapeo exponencial de $\mathbb{R}^3$ a SO(3) utilizo la fórmula de Rodrigues (que es el método estándar, creo).

El algoritmo se "desliza" por el gradiente de una función de error, que se rige por la pose $p$ es decir $$E(p)=\sum_i(I_1(x'(x_i;p))-I_0(x_i))^2$$ donde $x'(\cdot)$ es la función que mapea los píxeles de un espacio de imagen al otro espacio de imagen con respecto a la pose actual $p$ .

Para calcular el gradiente de $E(p)$ Tendré que calcular el gradiente de las matrices de rotación, lo que me lleva a mi verdadero problema.

Answer 1

No creo que necesites el jacobiano general $\frac{\partial}{\partial \mathbf p}\exp(\hat{\mathbf p})$ pero sólo el jacobiano mucho más simple $\left.\frac{\partial}{\partial \mathbf p}\exp(\hat{\mathbf p})\right|_{\mathbf p=\mathbf 0}$ con $\mathbf p$ estar en la identidad.

Antecedentes

El grupo de rotaciones 3d (SO3) es un grupo matricial de mentira. Por lo tanto, en general tenemos la exponencial matricial:

$$\exp(\mathtt M) := \sum_{k\ge 0} \frac{1}{k!} \mathtt M^k$$

que mapea un elemento del álgebra de Lie matricial en el conjunto de los elemetos del grupo de Lie matricial. Además, tenemos una función $$\hat \cdot: \mathbb R^n \rightarrow \mathbb R^{m\times m}, \quad \hat{\mathbf a} = \sum_{k=0}^n a_i\mathtt G_i$$ que mapea un $n$ -en el conjunto de elementos del álgebra de Lie matricial (=matrices). Así, para SO3 los generadores son: $$\mathtt G_1 = \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{bmatrix},\quad \mathtt G_2 =\begin{bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0 \end{bmatrix},\quad \mathtt G_3 =\begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}$$

Derivada en la identidad

Para los grupos matriciales de Lie, se puede demostrar que $$\left.\frac{\partial}{\partial a_k}\exp(\hat {\mathbf a})\right|_{\mathbf a=\mathbf 0} = \mathtt G_k.$$

Así, para el SO3 $$\left.\frac{\partial}{\partial \mathbf p}\exp(\hat {\mathbf p})\right|_{\mathbf p=\mathbf 0} = \begin{bmatrix}\mathtt G_1 & \mathtt G_2 & \mathtt G_3\end{bmatrix}$$ recibimos un $3$ d vector de filas de $3\times 3$ matrices, por lo que a $3\times 3 \times 3$ tensor jacobiano. (Como alternativa, se podrían apilar las columnas de $\mathtt G_i$ en un vector 9 y se recibiría un $9\times 3$ matriz jacobiana).

Antecedentes: Optimización por mínimos cuadrados

Supongamos que queremos minimizar la función $F:\mathbb R^n \rightarrow \mathbb R$ con respecto a un vector euclidiano $\mathbf x$ . Además, supongamos que tenemos un problema de mínimos cuadrados con

$$F = f(\mathbf x)^\top f(\mathbf x)$$

Siguiendo a Gauss-Newton resolvemos repetidamente una actualización $\delta$ $$\frac{\partial f}{\partial \mathbf x^{(m)}}^\top\frac{\partial f}{\partial \mathbf x^{(m)}} \delta = -\frac{\partial f}{\partial \mathbf x^{(m)}}^\top f(\mathbf x^{(m)})$$ y actualizar nuestra estimación $$\mathbf x^{(m+1)}=\delta + \mathbf x^{(m)}$$

Optimización por mínimos cuadrados en grupos matriciales de Lie

Este esquema sólo es válido para los espacios vectoriales euclidianos y es necesario adaptarlo para los grupos matriciales de Lie. Especialmente, calculamos la derivada en el espacio tangente alrededor de la identidad:

$$\mathtt J :=\left.\frac{\partial f(\exp(\hat{\mathbf p})\cdot\mathtt R^{(m)})}{\partial \mathbf p} \right|_{\mathbf p =\mathbf 0}$$ (Se puede calcular a partir de los generadores y utilizando la regla de la cadena).

Entonces resolvemos para $\delta$ :

$$\mathtt J^\top\mathtt J \delta = -\mathtt J^\top f(\mathtt R^{(m)})$$

Por último, adaptamos la regla de actualización: $$\mathtt R^{(m+1)}= \exp(\hat\delta)\cdot \mathtt R^{(m)}.$$

Answer 2

El enfoque de la optimización por mínimos cuadrados en los grupos matriciales de Lie también se explica en un informe técnico sobre Minimización en el grupo de Lie SO(3) y en las variedades relacionadas .

Answer 3

Un trabajo reciente de Guillermo Gallego y Anthony Yezzi sugiere una fórmula compacta para derivar la matriz de rotación en las coordenadas del mapa exponencial: http://arxiv.org/pdf/1312.0788v1.pdf

La fórmula (III.7) de la página 5 es lo que estabas buscando.

Answer 4

1. pregunta: No es una respuesta completa, pero ¿no debería derivar para $p$ y no $\omega$ ? Pero tal vez pueda hacer la derivada para $\omega$ como paso previo, y luego derivar esto para $p$

2. pregunta: El jacobiano no es un $9\times 4$ porque los 4 valores de una representación de 4 componentes no son independientes entre sí. O bien se toma un eje arbitrario con un ángulo - esto no sería único - o bien se toma un vector unitario con un ángulo - entonces los vectores-componentes no pueden ser elegidos libremente, ya que tienen que cumplir $\sqrt{x^2+y^2+z^2}=1$ . Así que sólo se necesitan 3 parámetros independientes para representar una rotación. Por lo tanto, el jacobiano es $9\times 3$ y no $9\times 4$ .

Answer 5

En este punto estoy bastante seguro de haber computado los tres primeros elementos de $J_R$ correctamente. Así que puedo seguir calculando los elementos restantes de la matriz.

Una forma de confirmar que el gradiente analítico es correcto, es mirar el gradiente numérico y luego comparar los valores entre ambos. Para ello, puede comprobar la función gradient() en Matlab u Octave.

El script que escribí es algo así:

S=1.
[x,y,z]=meshgrid(-2*pi:S:2*pi);
v=f(x,y,z); % this function is the one at element R(0,0)
[dx,dy,dz]=df(x,y,z) % these are the three partial gradients above
[ndx,ndy,ndz]=gradient(v,S)
figure
hold on;
quiver3(x,y,z,ndx,ndy,ndz, 'color','red','LineWidth',1);
quiver3(x,y,z,dx,dy,dz, 'color','green','LineWidth',1);
hold off;
legend('numeric gradient','analytic gradient');

Si todas las flechas tienen aproximadamente la misma orientación y longitud, es un buen indicador de que los gradientes parciales derivados son correctos.