This question was raised a while ago(here), but since it has not been solved yet and solving the problem is incredibly useful for robotics and 3-D vision etc., here I raised it again.
- Suppose we have $\mathbf{T} = \begin{bmatrix}
\mathbf{R} & \mathbf{t} \\
\mathbf{0} & 1
\end{bmatrix} \in \mathbb{SE}(3)$, where $\mathbf{R} \in \mathbb{SO}(3)$ and $\mathbf{t}\in \mathbb{R}^3$, and $\mathbf{x} = \begin{bmatrix}\mathbf{u}^\top & \mathbf{\omega}^\top \end{bmatrix}^\top \in \mathfrak{se}(3)$, where $\mathbf{u}\in\mathbb{R}^3$ and $\mathbf{\omega} \in \mathfrak{so}(3)$ and $\mathbf{T}=\exp(\mathbf{x})$.
The problem is how to derive the gradient or Jacobian matrix of the $\mathbf{T}$ with respect to $\mathbf{x}$:
$$
\frac{\partial \mathbf{T}}{\partial \mathbf{x}} = ?
$$
- we can expand the question further to include the derivatives of $\mathbb{SIM}(3)$ with respect to $\mathbb{sim}(3)$, where $\mathbf{S} = \begin{bmatrix}
\exp(\lambda)\mathbf{R} & \mathbf{t} \\
\mathbf{0} & 1
\end{bmatrix} \in \mathbb{SE}(3) $, and its corresponding tangent vector is $\mathbf{\delta} = \begin{bmatrix}\mathbf{u}^\top& \mathbf{\omega}^
\top &\lambda\end{bmatrix}^\top$ and $\lambda\in\mathbb{R}$ is a scalar. More details are available here. The problem of calculating the derivatives of $\mathbb{SIM}(3)$ w.r.t. its $\mathfrak{sim}(3)$ is shown as:
$$
\frac{\partial \mathbf{S}}{\partial \mathbf{\delta}} = ?
$$ - The problem is also closely related to the problem of calculating the derivatives of matrix in $\mathbb{SO}(3)$ w.r.t. its exponential coordinates in $\mathfrak{so}(3)$ i.e. $\frac{\partial \mathbf{R}}{\partial \omega}$, where $\mathbf{R}\in\mathbb{SO}(3)$, $\omega\in\mathfrak{so}(3)$, and $\mathbf{R}=\exp([\omega]_\times)$. Luckily, unlike previous two problems where computational cost was not considered when deriving the derivatives, Gallego and Yezzi provide a neat and compact solutions. Briefly, when $||\omega||\approx0$:
$$
\frac{\partial \mathbf{R}}{\partial \mathbf{\omega}} = \begin{bmatrix}[\mathbf{e}_1]_\times \\
[\mathbf{e}_2]_\times \\
[\mathbf{e}_3]_\times
\end{bmatrix},
$$
where $e_i$ is the $i$-th column of an $\mathbf{I}$ identity matrix.
otherwise,
$$
\frac{\partial \mathbf{R}}{\partial \omega_i} = \frac{\omega_i[\mathbf{\omega}]_\times+[\omega\times(\mathbf{I}-\mathbf{R})\mathbf{e}_i]_\times}{||\omega||^2}\mathbf{R}$$
Best Answer
Solution to problem 1
Inspired by Gallego's paper and Eade's notes about Lie Groups, I tried to derive a solution to my own problem 1 (correct me please if you see any error, thanks in advance):
According to Eqn (64) in Eade's notes, we have $\mathbf{T}=\begin{bmatrix}\exp([\omega]_\times) & \mathbf{Wu}\\ \mathbf{0} & 1 \end{bmatrix}=\begin{bmatrix}\mathbf{R} & \mathbf{Wu}\\ \mathbf{0} & 1 \end{bmatrix}$ where $\mathbf{W}=\mathbf{Id}+\frac{1-\cos\theta}{\theta^2}[\omega]_\times+\frac{\theta-\sin\theta}{\theta^3}[\omega]^2_\times$ and $\theta = ||\omega||$. Let's take a look at the partial derivatives w.r.t. to $\mathbf{t}$ first, $$ \frac{\partial \mathbf{T}}{\partial t_i} = \begin{bmatrix}\frac{\partial \mathbf{R}}{\partial t_i} & \frac{\partial \mathbf{Wu}}{\partial t_i}\\ \mathbf{0} & 1 \end{bmatrix}, $$ where $u_i$ is the $i$-th component of $\mathbf{u}$. It can be seen that $\frac{\partial \mathbf{R}}{\partial u_i}=\mathbf{0}$, since the rotation matrix has nothing to do with translation. $$ \frac{\partial \mathbf{Wu}}{\partial u_i}=\mathbf{W}\frac{\partial \mathbf{u}}{\partial u_i}=\mathbf{W}, $$ Then we can take a look at the partial derivatives w.r.t. to $\mathbf{\omega}$,
$$ \frac{\partial \mathbf{T}}{\partial \omega_i} = \begin{bmatrix}\frac{\partial \mathbf{R}}{\partial \omega_i} & \frac{\partial \mathbf{Wu}}{\partial \omega_i}\\ \mathbf{0} & 1 \end{bmatrix}, $$ where $\omega_i$ is the $i$-th component from $\omega$.
It can be seen that we can apply problem 3. for $\frac{\partial \mathbf{R}}{\partial \omega_i}$, and $\frac{\partial \mathbf{Wu}}{\partial \omega_i}=\frac{\partial \mathbf{W}}{\partial \omega_i}\mathbf{u}$. Now problem becomes how to solve $\frac{\partial \mathbf{W}}{\partial \omega_i}$.
$$ \begin{align*} \frac{\partial \mathbf{W}}{\partial \omega_i}&=\frac{\partial (\mathbf{Id}+\frac{1-\cos\theta}{\theta^2}[\omega]_\times+\frac{\theta-\sin\theta}{\theta^3}[\omega]^2_\times)}{\partial \omega_i}\\ &=\frac{\partial(\frac{1-\cos\theta}{\theta^2})}{\partial \omega_i}[\omega]_\times+ \frac{1-\cos\theta}{\theta^2}\frac{\partial [\omega]_\times}{\partial \omega_i} + \frac{\partial(\frac{\theta-\sin\theta}{\theta^3})}{\partial \omega_i}[\omega]^2_\times+ \frac{\theta-\sin\theta}{\theta^3}\frac{\partial [\omega]^2_\times}{\partial \omega_i}\\ &=(\frac{-2(1-\cos\theta)}{\theta^3}+\frac{\sin\theta}{\theta^2})\frac{\partial \theta}{\partial \omega_i}[\omega]_\times+(\frac{1-\cos\theta}{\theta^2})[e_i]_\times+(\frac{-3(\theta-\sin\theta)}{\theta^4}+\frac{1-\cos\theta}{\theta^3})\frac{\partial \theta}{\partial \omega_i}[\omega]^2_\times+\frac{\theta-\sin\theta}{\theta^3}(\mathbf{e}_i\mathbf{v}^\top+\mathbf{v}^\top\mathbf{e}_i-2\omega_i\mathbf{I})\\ &=(\frac{-2(1-\cos\theta)}{\theta^3}+\frac{\sin\theta}{\theta^2})\frac{\omega_i}{\theta}[\omega]_\times+(\frac{1-\cos\theta}{\theta^2})[e_i]_\times+(\frac{-3(\theta-\sin\theta)}{\theta^4}+\frac{1-\cos\theta}{\theta^3})\frac{\omega_i}{\theta}[\omega]^2_\times+\frac{\theta-\sin\theta}{\theta^3}(\mathbf{e}_i\mathbf{v}^\top+\mathbf{v}^\top\mathbf{e}_i-2\omega_i\mathbf{I}) \end{align*} $$
Similar to the solution to $\mathbb{SO}(3)$, when $||\omega||\approx0$ the $\frac{\partial \mathbf{W}}{\partial\omega_i}$ becomes: $$ \frac{\partial \mathbf{W}}{\partial\omega_i}=[\mathbf{e}_i]_\times $$
This derivation should be correct, but I am really not happy about its computational costs and it looks ugly as well in contrast to the beautiful soltion to SO(3) derived by Gallego et al.. Another cue might be useful to professionals is that according to Hausdorff formula the derivative of an exponential map should fulfill:
$$ \frac{\partial \exp(\mathbf{X})}{\partial \mathbf{t}}=\mathbf{X}_d\exp(\mathbf{X}), $$ where $\mathbf{X}_d=\frac{\partial \mathbf{x}}{\partial t}+\frac{1}{2!}[\mathbf{X},\frac{\mathbf{x}}{\partial t}]+\frac{1}{3!}[\mathbf{X}, [\mathbf{X},\frac{\mathbf{x}}{\partial t}]] +...$ where $[.]$ is the Lie bracket operation (Page 73 from (1)). For example, in problem 3, we can see that $$ \frac{\partial \mathbf{R}}{\partial \omega}=\mathbf{R}_d\mathbf{R}, $$ where $\mathbf{R}_d=\frac{\omega_i[\mathbf{\omega}]_\times+[\omega\times(\mathbf{Id}-\mathbf{R})\mathbf{e}_i]_\times}{||\omega||^2}$ similarly, the derivative of exponential map of $\mathbb{SO}(3)$ and $\mathbb{SIM}(3)$ should have the same format: $$ \frac{\partial \mathbf{T}}{\partial \omega}=\mathbf{T}_d\mathbf{T}=\begin{bmatrix}\mathbf{R}_d\mathbf{R} & \frac{\partial \mathbf{W}}{\partial \omega_i}\mathbf{W}^{-1}\mathbf{Wu} \\ \mathbf{0} & 1 \end{bmatrix} = \begin{bmatrix}\mathbf{R}_d & \frac{\partial \mathbf{W}}{\partial \omega_i}\mathbf{W}^{-1} \\ \mathbf{0} & 1 \end{bmatrix}\begin{bmatrix}\mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{bmatrix}= \begin{bmatrix}\mathbf{R}_d & \frac{\partial \mathbf{W}}{\partial \omega_i}\mathbf{W}^{-1} \\ \mathbf{0} & 1 \end{bmatrix}\mathbf{T} $$
Solving problem 2.
According to Eqn(17) to (20) in Spyridon's ICRA15 paper, we have $$\mathbf{S}=\begin{bmatrix}\exp(\lambda+[\omega]_\times) & \mathbf{Vu}\\ \mathbf{0}& 1\end{bmatrix}=\begin{bmatrix}\exp(\lambda)\mathbf{R} & \mathbf{Vu}\\\mathbf{0}&1\end{bmatrix},$$ where
1) $\mathbf{V}=\mathbf{i}$ if $\theta=0$ and $\lambda=0$;
2) $\mathbf{V}= \frac{\exp(\lambda)-1}{\lambda}\mathbf{I}$ if $\theta=0$ but $\lambda\neq0$;
3) $\mathbf{V}=\mathbf{W}$ if $\theta\neq0$ and $\lambda=0$.
4) $\mathbf{V}=\frac{\exp(\lambda)-1}{\lambda}\mathbf{I}+\frac{\theta(1-\exp(\lambda)\cos\theta)+\exp(\lambda)\lambda\sin\theta}{\theta(\lambda^2+\theta^2)}[\omega]_\times+\left ( \frac{\exp(\lambda)-1}{\lambda\theta^2}-\frac{\exp(\lambda)\sin\theta}{\theta(\lambda^2+\theta^2)} -\frac{\lambda(\exp(\lambda)\cos\theta-1)}{\theta^2(\lambda^2+\theta^2)} \right )[\omega]_\times^2$, if $\theta\neq0$ but $\lambda\neq0$;
Now, let's take a look at the derivatives in $\mathbf{u}$ first:
(to be continued)
(1) Bruno Siciliano, O. khatib(Eds. . (2008). Geometric Fundamentals of Robotics.
(2) Drummond, T. (2014). Lie groups, Lie algebras, projective geometry and optimization for 3D Geometry, Engineering and Computer Vision.
(3) Eade, E. (2013). Lie Groups for 2D and 3D Transformations, 1–24.
(4) Gallego, G., & Yezzi, A. (2015). A Compact Formula for the Derivative of a 3-D Rotation in Exponential Coordinates. Journal of Mathematical Imaging and Vision, 51(3), 378–384.
(5) Leonardos, S., Blanchette, C. A., & Gallier, J. (2015). The exponential map for the group of similarity transformations and applications to motion interpolation. In ICRA (Vol. 1).
Cave
greg
Andrea
hans