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 {{citation}}: Empty citation (help): 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: