I am working an algorithm which is supposed to align a pair of images. The motion model, which describes the pose $p$ of an image (with respect to the second) in 3D space, is purely rotational.
Theory to my question:
According to the Rodrigues' formula for rotation matrices, the matrix exponential $R(p)=e^{\hat{p}}$ for a vector $p=(p_x,p_y,p_z)^T\in\mathbb{R}^3$ is given by
$$
\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}
$$
where $||p||=\sqrt{p_x^2+p_y^2+p_z^2}$ and
$$
\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}.
$$
Summing up the elements of $R(p)$ yields
$$
\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}
$$
where $r=\frac{\sin(||p||)}{||p||}$ and $s=\frac{(1-\cos(||p||))}{||p||^2}$ were introduced for clarity.
Now, let $R^s(p)\in\mathbb{R}^{9\times 1}$ be the stacked version of $R(p)$, i.e. all columns are stacked into one column and let $R^s_i(p)$ denote the $i$-th element of $R^s(p)$ where $1\leq i \leq 9$.
The Jacobian matrix $J_{R}\in\mathbb{R}^{9\times 3}$ with respect to vector $p$ is then given by
$$
\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}
$$
The first element of $J_R$ can then be expressed as
$$
\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}
$$
the second one as
$$
\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}
$$
and the third element of the first rowas
$$
\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}
$$
Already a big thanks for reading this far 😉
My question: Is the derivation of these first 3 elements of $J_R$ mathematically correct? If I did something wrong, can you spot the error(s) and possibly help me correct it(/them)?
Once I have got everything straight, I will post the complete Jacobian matrix as the answer to this question. So anyone confronted with the same problem can double-check his own results …
The reason I need this Jacobian:
The pose $p$ of an image in the application mentioned above can be described by an unit rotation axis and an angle (in radian). Alternatively, the unit axis can be scaled according to the rotation angle, which is what I am using. To do the exponential mapping from $\mathbb{R}^3$ to SO(3) I use the Rodrigues' formula (which is the standard method, I think).
The algorithm "slides" down the gradient of an error function, which is governed by the pose $p$, i.e.
$$E(p)=\sum_i(I_1(x'(x_i;p))-I_0(x_i))^2$$
where $x'(\cdot)$ is the function that maps pixels from one image space to the other image space with respect to the current pose $p$.
In order to compute the gradient of $E(p)$ I will have to compute the gradient of the rotation matrices, which leads to my actual problem.
Best Answer
I don't think you need the general Jacobian $\frac{\partial}{\partial \mathbf p}\exp(\hat{\mathbf p})$, but only the much simpler Jacobian $\left.\frac{\partial}{\partial \mathbf p}\exp(\hat{\mathbf p})\right|_{\mathbf p=\mathbf 0}$ with $\mathbf p$ being at the identity.
Background
The group of 3d rotations (SO3) is a matrix lie group. Thus, in general we have the matrix exponential:
$$\exp(\mathtt M) := \sum_{k\ge 0} \frac{1}{k!} \mathtt M^k$$
which maps an element of the matrix Lie algebra onto the set of the matrix Lie group elemets. Furthermore we have a function $$\hat \cdot: \mathbb R^n \rightarrow \mathbb R^{m\times m}, \quad \hat{\mathbf a} = \sum_{k=0}^n a_i\mathtt G_i$$ which maps an $n$-vector onto the set of matrix Lie algebra elements (=matrices). Thus, for SO3 the generators are: $$ \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} $$
Derivative at the identity
For matrix Lie groups, it can be shown that $$ \left.\frac{\partial}{\partial a_k}\exp(\hat {\mathbf a})\right|_{\mathbf a=\mathbf 0} = \mathtt G_k.$$
Thus for 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}$$ we receive a $3$d row vector of $3\times 3$ matrices, hence a $3\times 3 \times 3$ Jacobian tensor. (Alternatively, one could stack the columns of $\mathtt G_i$ into a 9-vectors and one would receive a $9\times 3$ Jacobian matrix.)
Background: Least square optimization
Let us assume we would like to minimize the function $F:\mathbb R^n \rightarrow \mathbb R$ with respect to a Euclidean vector $\mathbf x$. Furthermore let use assume we have a least square problem with
$$F = f(\mathbf x)^\top f(\mathbf x)$$
Following Gauss-Newton we repeatedly solve for an update $\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)})$$ and update our estimate $$ \mathbf x^{(m+1)}=\delta + \mathbf x^{(m)}$$
Least square optimization on matrix Lie groups
This scheme is only valid for Euclidean vector spaces and need to be adapted for matrix Lie groups. Especially, we calculate the derivative in the tangent space around the identity:
$$\mathtt J :=\left.\frac{\partial f(\exp(\hat{\mathbf p})\cdot\mathtt R^{(m)})}{\partial \mathbf p} \right|_{\mathbf p =\mathbf 0}$$ (Can be calculated from the generators and using the chain rule.)
Then we solve for $\delta$:
$$ \mathtt J^\top\mathtt J \delta = -\mathtt J^\top f(\mathtt R^{(m)})$$
Finally we adapt the update rule: $$ \mathtt R^{(m+1)}= \exp(\hat\delta)\cdot \mathtt R^{(m)}.$$