1. so(3)

1.1 회전 행렬 R과의 관계

exponential maps

so(3), axis angle인 $\phi$는 axis $u$와 회전 크기 angle $\theta$의 곱으로,
Rodrigues 공식에 따르면
$R=exp([\phi]_\times)=exp(\theta[u]_\times)=I+sin(\theta)[u]_\times+(1-cos(\theta))[u]^2_\times$

where $[v]_\times=\begin{pmatrix}0&-v_3&v_2\\v_3&0&-v_1\\-v_2&v_1&0\end{pmatrix}$

logarithmic maps

$\theta=arccos((tr(R)-1)/2)$
$u=(R-R^T)^{\lor}/(2sin(\theta))$

vector rotation

$x'=Rx$

rotation composition

$R_{AC}=R_{AB}R_{BC}$

spherical linear interpolation

$R(t)=R(0)(R(0)^TR(1))^t=R(0)(I+sin(t\Delta\theta)[u]_\times+(1-cos(t\Delta\theta))[u]^2_\times)$

Right/Left Jacobian of SO(3)

$J_r(\phi)=\frac{\partial R\{\phi\}}{\partial \phi}=I-\frac{1-cos||\phi||}{||\phi||^2}[\phi]_\times+\frac{||\phi||-sin||\phi||}{||\phi||^3}[\phi]^2_\times$
$J_r^{-1}(\phi)=I+\frac{1}{2}[\phi]_\times+(\frac{1}{||\phi||^2}-\frac{1+cos||\phi||}{2||\phi||sin||\phi||})[\phi]^2_\times$

$J_r(\phi)=J_l(-\phi)\in R^{3\times3}$

examples of usage of Jacobians of SO(3)

$R\{\phi+\delta \phi\}\approx R\{\phi\}R\{J_r(\phi)\delta \phi\}$

$R\{\phi\}R\{\delta \phi\} \approx R\{\phi+J_r^{-1}(\phi)\delta \phi\}$

Jacobian wrt rotation vector

$\frac{\partial R\{\phi\}x}{\partial \delta\phi}=-R\{\phi\}[x]_\times J_r(\phi)=-[R\{\phi\}x]_\times J_l(\phi)$

perturbation

$\frac{\partial(Rx)}{\partial\varphi}=-[Rx]_\times$

1-2 유닛 쿼터니언 q와의 관계

exponential maps

$q=Exp(\phi)=exp(\phi/2)=\begin{bmatrix}cos(\phi/2)\\u*sin(\phi/2)\end{bmatrix}$

logarithmic maps

$\theta=2arctan(||q_v||, q_w)$
$u=q_v/||q_v||$

vector rotation

$x'=q\otimes x\otimes q^*=[q]_L[q^*]_Rx$

where $[q]_L=\begin{pmatrix}q_w&-q_x&-q_y&-q_z\\q_x&q_w&-q_z&q_y\\q_y&q_z&q_w&-q_x\\q_z&-q_y&q_x&q_w\end{pmatrix},$

$[q]_R=\begin{pmatrix}q_w&-q_x&-q_y&-q_z\\q_x&q_w&q_z&-q_y\\q_y&-q_z&q_w&q_x\\q_z&q_y&-q_x&q_w\end{pmatrix}$

rotation composition

$q_{AC}=q_{AB}\otimes q_{BC}$

where $p\otimes q=\begin{pmatrix}p_wq_w-p_v^Tq_v\\p_wq_v+q_wp_v+p_v\times q_v\end{pmatrix}$

spherical linear interpolation

$\Delta\theta=arccos(q(0)^Tq(1))$

$q(t)=q(0)\frac{sin((1-t)\Delta\theta)}{sin(\Delta\theta)}+q(1)\frac{sin(t\Delta\theta)}{sin(\Delta\theta)}$

Jacobian wrt quaternion

$\frac{\partial (q\otimes x\otimes q^*)}{\partial q}=2\begin{pmatrix}q_wx+q_v\times x&|&q_v^TxI_3+q_vx^T-xq_v^T-q_w[x]_\times\end{pmatrix}\in R^{3\times4}$

2. se(3)

$T=\begin{pmatrix}R&t\\0^T&1\end{pmatrix}\in R^{4\times4}$

$\xi=\begin{pmatrix}\rho\\\phi\end{pmatrix}\in R^6$

exponential maps

$exp([\xi]_\times)=\begin{pmatrix}exp([\phi]_\times)&J\rho\\0^T&1\end{pmatrix}=T$

where $J(\phi)=\frac{sin(\theta)}{\theta}I+(1-\frac{sin(\theta)}{\theta})uu^T+\frac{1-cos(\theta)}{\theta}[u]_\times$

logarithmic maps

$\rho=J^{-1}t,\quad\phi=ln(R)^\lor$

Right/Left Jacobian of SE(3)

$\mathcal{J}_r(\xi)=\frac{\partial T\{\xi\}}{\partial \xi}=\begin{pmatrix}J_r(\phi)&Q_r(\xi)\\0_{3\times3}&J_r(\phi)\end{pmatrix}\in R^{6\times6}$

where $Q_r(\xi)=-\frac{1}{2}[\rho]_\times+(\frac{\theta-sin(\theta)}{\theta^3})([\phi]_\times[\rho]_\times+[\rho]_\times[\phi]_\times-[\phi]_\times[\rho]_\times[\phi]_\times) - (\frac{\theta^2+2cos(\theta)-2}{2\theta^4})([\phi]_\times^2[\rho]_\times+[\rho]_\times[\phi]^2_\times-3[\phi]_\times[\rho]_\times[\phi]_\times)+(\frac{2\theta-3sin(\theta)+\theta cos(\theta)}{2\theta^5})([\phi]_\times[\rho]_\times[\phi]^2_\times+[\phi]^2_\times[\rho]_\times[\phi]_\times)$

where $(\theta=||\phi||)$

$\mathcal{J}_r^{-1}(\xi) \approx I_{6\times6}+\frac{1}{2}\begin{pmatrix}[\phi]_\times&[\rho]_\times\\0_{3\times3}&[\phi]_\times\end{pmatrix}$

examples of usage of Jacobians of SE(3)

$T\{\xi+\delta \xi \}\approx T\{\xi\}T\{\mathcal{J}_r(\xi) \delta \xi \}$
$T\{\xi\}T\{\delta \xi\}\approx T\{\xi + \mathcal{J}_r^{-1}(\xi)\delta \xi \}$

perturbation

$\frac{\partial(Tp)}{\partial\delta\xi}=\begin{pmatrix}I_3&-[Rp+t]_\times\\0^T&0^T\end{pmatrix}\in R^{4\times6}$