3.2 Exponential Map

The exponential map from $\mathrm{se}(3)$ to $\mathrm{SE}(3)$ is the matrix exponential on a linear combination of the generators:

$\displaystyle \boldsymbol{\delta}=\left(\begin{array}{cc} \mathbf{u} & \boldsymbol{\omega}\end{array}\right)$	$\displaystyle \in$	$\displaystyle \mathrm{se}(3)$	(61)
$\displaystyle \exp\left(\boldsymbol{\delta}\right)$	$\displaystyle =$	$\displaystyle \exp\left(\begin{array}{c\vert c} \boldsymbol{\omega}_{\times} & \mathbf{u}\\ \hline \mathbf{0} & 0 \end{array}\right)$	(62)
	$\displaystyle =$	$\displaystyle \mathbf{I}+\left(\begin{array}{c\vert c} \boldsymbol{\omega}_{\ti... ...mega}_{\times}^{2}\mathbf{u}\\ \hline \mathbf{0} & 0 \end{array}\right)+\cdots$	(63)

The rotation block is the same as for $\mathrm{SO}(3)$ , but the translation component is a different power series:

$\displaystyle \exp\left(\begin{array}{c\vert c} \boldsymbol{\omega}_{\times} & \mathbf{u}\\ \hline \mathbf{0} & 0 \end{array}\right)$	$\displaystyle =$	$\displaystyle \left(\begin{array}{c\vert c} \exp\left(\boldsymbol{\omega}_{\times}\right) & \mathbf{V}\mathbf{u}\\ \hline \mathbf{0} & 1 \end{array}\right)$	(64)
$\displaystyle \mathbf{V}$	$\displaystyle =$	$\displaystyle \mathbf{I}+\frac{1}{2!}\boldsymbol{\omega}_{\times}+\frac{1}{3!}\boldsymbol{\omega}_{\times}^{2}+\cdots$	(65)

Again using the identity from Eq. 9, we split the terms by odd and even powers, and factor out :

$\displaystyle \mathbf{V}$	$\displaystyle =$	$\displaystyle \mathbf{I}+\sum_{i=0}^{\infty}\left[\dfrac{\boldsymbol{\omega}_{\... ...s}^{2i+1}}{(2i+2)!}+\dfrac{\boldsymbol{\omega}_{\times}^{2i+2}}{(2i+3)!}\right]$	(66)
	$\displaystyle =$	$\displaystyle \mathbf{I}+\left(\sum_{i=0}^{\infty}\dfrac{(-1)^{i}\theta^{2i}}{(... ...fty}\dfrac{(-1)^{i}\theta^{2i}}{(2i+3)!}\right)\boldsymbol{\omega}_{\times}^{2}$	(67)

The coefficients can be identified with Taylor expansions, yielding a formula for $\mathbf{V}$ :

$\displaystyle \mathbf{V}$	$\displaystyle =$	$\displaystyle \mathbf{I}+\left(\frac{1}{2!}-\frac{\theta^{2}}{4!}+\frac{\theta^... ...ta^{2}}{5!}+\frac{\theta^{4}}{7!}+\cdots\right)\boldsymbol{\omega}_{\times}^{2}$	(68)
	$\displaystyle =$	$\displaystyle \mathbf{I}+\left(\dfrac{1-\cos\theta}{\theta^{2}}\right)\boldsymb... ...ft(\dfrac{\theta-\sin\theta}{\theta^{3}}\right)\boldsymbol{\omega}_{\times}^{2}$	(69)

$\displaystyle \mathbf{u},\boldsymbol{\omega}$	$\displaystyle \in$	$\displaystyle \mathbb{R}^{3}$	(70)
$\displaystyle \theta$	$\displaystyle =$	$\displaystyle \sqrt{\boldsymbol{\omega}^{T}\boldsymbol{\omega}}$	(71)
$\displaystyle A$	$\displaystyle =$	$\displaystyle \dfrac{\sin\theta}{\theta}$	(72)
$\displaystyle B$	$\displaystyle =$	$\displaystyle \dfrac{1-\cos\theta}{\theta^{2}}$	(73)
$\displaystyle C$	$\displaystyle =$	$\displaystyle \dfrac{1-A}{\theta^{2}}$	(74)
$\displaystyle \mathbf{R}$	$\displaystyle =$	$\displaystyle \mathbf{I}+A\boldsymbol{\omega}_{\times}+B\boldsymbol{\omega}_{\times}^{2}$	(75)
$\displaystyle \mathbf{V}$	$\displaystyle =$	$\displaystyle \mathbf{I}+B\boldsymbol{\omega}_{\times}+C\boldsymbol{\omega}_{\times}^{2}$	(76)
$\displaystyle \exp\left(\begin{array}{c} \mathbf{u}\\ \boldsymbol{\omega} \end{array}\right)$	$\displaystyle =$	$\displaystyle \left(\begin{array}{c\vert c} \mathbf{R} & \mathbf{V}\mathbf{u}\\ \hline \mathbf{0} & 1 \end{array}\right)$	(77)

For implementation purposes, Taylor expansions of

, and

should be used when $\theta^{2}$ is small.

The $\ln()$ function on $\mathrm{SE}(3)$ can be implemented by first finding $\ln(\mathbf{R})$ as shown in Eq. 18, then constructing $\mathbf{V}$ , and finally solving $\mathbf{Vu}=\mathbf{t}$ for $\mathbf{u}$ (e.g. using Gaussian elimination with partial pivoting).