User:BenFrantzDale/SE(3)

The Euclidean group in 3-dimensional space is known as SE(3)—the group of rigid-body motions in 3D. This has applications in kinematics, computer vision, robotics, etc. SE(3) is a Lie group, and has a corresponding Lie algebra representing the tangent space at the identity—the space of rigid-body velocities.

This group is generally represented as a homogeneous transformation matrix: a 4×4 matrix with the top-left 3×3 block a rotation matrix—an element of SO(3), the top-right 3×1 block a translation vector, and the bottom row being [0 0 0 1].

Exponential map
The exponential map on SE(3) is given by the matrix exponential and matrix logarithm. The logarithm turns the rotation part into a cross product matrix corresponding to the axis-angle representation of the rotation.

Exp
To compute
 * $$\begin{bmatrix}R &t \\0 & 1\end{bmatrix}=\exp\begin{bmatrix}\omega \\v \end{bmatrix}$$:

let
 * $$S = \begin{bmatrix}[\omega]_\times & v\\ 0 & 0\end{bmatrix}$$

if $$|\omega| = 0$$, there is no rotation and the result is $$I+S$$. Otherwise, the result is:
 * $$\exp\begin{bmatrix}\omega \\t \end{bmatrix}= I + S + \frac{1-\cos|\omega|}{|\omega|^2} S^2 + \frac{|\omega|-\sin|\omega|}{|\omega|^3} S^3$$.

In practice the full equation breaks down numerically near $$|\omega|=0$$ and so terms should be replaced by a polynomial expansion.

Alternately, we can replace the S expressions with more-basic identities: From Rodrigues' rotation formula we have:
 * $$R = I + \sin\theta [\mathbf{k}]_\times + (1 - \cos\theta) (\mathbf{k} \mathbf{k}^\top-I)$$ where $$\mathbf{k}:=\omega/\|\omega\|=\omega/\theta$$.

And then compute t:
 * $$t = \frac{(I-R) \omega \times v + v \cdot \omega \omega}{\theta^2}$$

which simplifies to
 * $$t = \frac{(I-R) [\omega]_\times + \omega \omega^\top}{\theta^2} v$$.

Note that
 * $$I-R = I-I - \sin\theta [\mathbf{k}]_\times - (1 - \cos\theta) (\mathbf{k} \mathbf{k}^\top-I)$$
 * $$I-R = -\sin\theta [\mathbf{k}]_\times - (1 - \cos\theta) (\mathbf{k} \mathbf{k}^\top-I)$$

and so
 * $$(I-R)[\omega]_\times = \frac{-\sin\theta}{\theta} [\omega]_\times^2 + (1-\cos\theta)(\mathbf{k}\mathbf{k}^\top [\omega]_\times-[\omega]_\times)$$
 * $$(I-R)[\omega]_\times = \frac{-\sin\theta}{\theta} [\omega]_\times^2 + (1-\cos\theta)[\omega]_\times$$

Log
To compute $$\begin{bmatrix}\omega \\v \end{bmatrix}=\log\begin{bmatrix}R &t \\0 & 1\end{bmatrix}$$: First,
 * $$\omega:=\log(R)$$

according to the Rodrigues' rotation formula. Note that there are numerical issues around zero and around $$\theta=\pi$$. If $$|\omega|=0$$ (no rotation) then $$v=t$$ and we are done. Otherwise, it's complicated:

We first find the screw representation of the transformation. We have the $$\omega$$ part, so we need to find a point, u, on the screw axis. The action of the transformation, of course, will move points in a screw motion about that axis...

We'll transform a point, p, and take the perpendicular bisector to the segment between $$p$$ and $$p$$ where $$p$$ is p transformed projected back onto the plane through x that intersects p. Let:
 * p be any point
 * $$p':= Rp+t$$
 * $$p'':=p'-((p'-p)\cdot \hat x)\hat x = p' \hat x \hat x^\top (p'-p) = (I-\hat x \hat x^\top) p' + \hat x \hat x^\top p$$

Let
 * $$b:= \frac{\|p-p\|}{2}$$, the distance from p to the middle of the line to $$p$$
 * $$a:= \frac{\|p-p\|}{2\tan\frac{\theta}{2}}$$, the height of the trangle formed by the center of rotation, p, and $$p$$.
 * $$\vec a:= \hat x \times(p''-p)$$
 * $$\bar u:= \frac{p+p}{2} + \frac{\|p-p\|}{2 \tan\frac{\theta}{2}} \cdot \frac{x\times(p-p)}{\|\hat x\times(p-p)\|}$$ where $$\overline u$$ is on the screw axis, not necessarily perpendicular to $$\hat x$$.

So in general,
 * $$u=\bar u + \alpha \hat x$$

and
 * $$ (\bar u+\alpha \hat x)\cdot \hat x = 0 \Rightarrow \alpha \hat x\cdot \hat x = -\bar u \cdot \hat x =\Rightarrow \alpha = -\frac{\bar u\cdot \hat x}{\hat x\cdot \hat x} = -\hat u \cdot \hat x$$

so if we enforce that u is perpendicular to $$\hat x$$, we have
 * $$u=\bar u - (\bar u \cdot \hat x)\hat x = (I-\hat x\hat x^\top) \bar u$$.

Since p is arbitrary, we can let it be the zero vector. Then we have
 * $$u=\left((I-\hat x\hat x^\top) + \frac{[\hat x]_\times}{\tan\frac{\theta}{2}}\right) t/2$$.

Now, the v component of the 6-vector we are looking for is not u, it is:
 * $$v=u\times \omega + \hat x \hat x^\top t = \hat x \hat x ^\top t - \omega \times u$$ (why?)

We can substitute to expand this out:
 * $$\omega\times u = \theta [\hat x]_\times u$$

since $$\omega$$ is the vector in the direction of the unit vector $$\hat x$$ with magnitude $$\theta$$. So we can plug in the u from above. Also, note that:
 * $$\hat x \hat x^\top = I +[\hat x]^2$$

and:
 * $$[\hat x]_\times \hat x \hat x^\top = 0$$

so we have
 * $$\omega \times u = \theta [\hat x]_\times u$$
 * $$\omega \times u = \theta [\hat x]_\times \left(I-\hat x \hat x^\top + \frac{[\hat x]_\times}{\tan \frac{\theta}{2}}\right)\frac{t}{2}$$
 * $$\omega \times u = \theta \left([\hat x]_\times + \frac{[\hat x]_\times^2}{\tan\frac{\theta}{2}}\right)\frac{t}{2}$$

So including the full expression for v, we get:
 * $$v=\left(-\frac{\theta}{2}\left([\hat x]_\times + \frac{[\hat x]_\times^2}{\tan\frac{\theta}{2}}\right) t + \hat x \hat x^\top t \right)$$
 * $$v=\left(-\frac{\theta}{2}\left([\hat x]_\times + \frac{[\hat x]_\times^2}{\tan\frac{\theta}{2}}\right) + \hat x \hat x^\top \right) t$$
 * $$v=\left(-\frac{\theta}{2}[\hat x]_\times + \frac{-\theta}{2 \tan\frac{\theta}{2}} + \hat x \hat x^\top \right) t$$
 * $$v=\left(I+\frac{-\theta}{2}[\hat x]_\times + \left(1-\frac{\theta}{2 \tan\frac{\theta}{2}}\right) [\hat x]_\times^2\right) t$$
 * $$v=\left(I-\frac{[\omega]_\times}{2} + \frac{1}{\theta^2} \left(1-\frac{\theta}{2 \tan\frac{\theta}{2}}\right) [\omega]_\times^2\right) t$$.

Finally, if you want to rearrange that into a form shown in other papers, you can apply the half angle formula for tangent:
 * $$\tan\frac{\theta}{2}=\frac{\sin\theta}{1+\cos\theta}$$

so
 * $$\frac{1}{\theta^2}\left(1-\frac{\theta(1+\cos \theta)}{2 \sin\theta}\right)$$
 * $$ = \frac{1}{\theta^2}\frac{2\sin\theta-\theta(1+\cos\theta)}{2\sin\theta}$$
 * $$ = \frac{2\sin\theta-\theta(1+\cos\theta)}{2\theta^2\sin\theta}$$

which is found in various references such as http://www.kramirez.net/Robotica/Material/Libros/A%20mathematical%20Introduction%20to%20Robotic%20manipulation.pdf p. 414.

Note: In practice the full equation breaks down numerically near $$|\omega|=0$$ and so terms should be replaced by a polynomial expansion.