Lie Group For Beginners

Abstract

This tutorial is to help those who have never used Lie Groups but want to use this useful tool. As the manifold topic is too large to cover in this single tutorial, we will concentrate those facts that are necessary for computing Jacobian matrix which will be used in future optimization. Some example codes will be given for each part of the tutorial. If you have any questions about this tutorial, please email to me.

Lie Group: SO(3) and SE(3)

1. SO(3): special orthogonal group.
2. SE(3): special euclidean group.

A group is associated with some space and the operations that can applied on the elements in this space. We are familiar with 3D Euclidean space (vector space). For example, $a = (1,2,3), b = (3,4,5)$ are elements of the vector space, the plus operator can be applied by $c=a+b$, which is also an element of the vector space. Obviously SO(3) is not in the vector space, as we cannot simply plus two rotation matrix. However, SO(3) and SE(3) belongs to the Lie Group, which has five properties. We use $C$ for SO(3) and $T$ for SE(3). Suppose $C_1,C_2,C_3\in SO(3)$ and $T_1,T_2,T_3 \in SE(3)$:

1. C,T are also a differential manifold.
2. $C_1C_2\in SO(3)$ and $T_1T_2\in SE(3)$.
3. Associativity: $(C_1C_2)C_3$ = $C_1(C_2C_3)$. $(T_1T_2)T_3=T_1(T_2T_3)$
4. Identity: $C_1I=IC_1=C_1$, $T_1I=IT_1=T_1$
5. Invertibility: $C_1^{-1}\in SO(3)$, $T_1^{-1}\in SE(3)$

Necessary and sufficient conditions fro SO(3): $C^TC = I, det(C) =1$, and for SE(3): $T=\begin{bmatrix} C,t\\0,1\end{bmatrix},where, C\in SO(3), t\in \mathbb{R}^3$

Lie Algebras

Every Lie group associates with a Lie Algebras, which is in vector space $\mathbb{V}$. More specifically, the Lie Algebras is skew matrix (a square real matrix (subspace) of vector space.), and with some operators Lie brackets.

1. Closure: $[X,Y]\in \mathbb{V}$
2. Bilinearity: [aX+bY,Z] = a[X,Z] +b[Y,Z]
3. Alternating: [X,X] = 0
4. Jacobi identity: [X,[Y,Z]]=[Z,[Y,X]]=[Y,[Z,X]]
   Above is the general form of Lie brackets for Lie Algebras. Next we will see two special Lie Algebras, $\mathfrak{so}(3),\mathfrak{se}(3)$.

Rotation RO(3)

The Lie algebra associated with SO(3) is given by
1. Vector space: $\mathfrak{so}(3) =\{\mathbf{\Phi}=\phi^\wedge\in \mathbb{R}^{3\times 3}|\phi \in \mathbb{R}^3\}$
2. Field: $\mathbb{R}$
3. Lie bracket:$[\mathbf{\Phi}_1,\mathbf{\Phi}_2]=\mathbf{\Phi}_1\mathbf{\Phi}_2-\mathbf{\Phi}_2\mathbf{\Phi}_1$
Based on Lie bracket defined above, we can easily tested that, the four properties (general Lie Algebras) holds.

Pose SE(3)

The Lie algebra associated with SE(3) is given by
1. Vector space, $\mathfrak{se}(3)=\{\Xi=\xi^\wedge \in \mathbb{R}^{4\times4}|\xi \in \mathbb{R}^6\}$
2. Field: $\mathbb{R}$
3. Lie bracket: $[\Xi_1,\Xi_2] =\Xi_1\Xi_2-\Xi_1\Xi_2$
where

$$\begin{equation} \xi^\wedge = \begin{bmatrix} \rho\\ \phi \end{bmatrix}^\wedge = \begin{bmatrix} \phi^\wedge, \rho\\ 0,1 \end{bmatrix} \in \mathbb{R}^{4\times4} \end{equation}$$

Exponential Map

We have defined Lie Group and Lie Algebras above, here we will study Exponential Map, that will connect those two concepts. \
The matrix exponential map and matrix logarithms are defined:

$$\begin{equation}\label{ep:exponentialA} \begin{split} exp(A) &= 1 + A + \frac{1}{2!}A^2 +\frac{1}{3!}A^3+... \\ ln(A) &= \sum_{n=1}^{\infty}\frac{(-1)^{n-1}}{n}(A-1)^n \end{split} \end{equation}$$

Rotation SO(3)

Let $C\in SO(3), \phi^\wedge \in \mathfrak{so}(3)$, according to Eq. $\ref{ep:exponentialA}$

$$\begin{equation}\label{eq:em:RSo3} \begin{split} C &= exp(\phi^\wedge) = \sum_{n=1}^{\infty} \frac{1}{n!}(\phi^\wedge)^n\\ \phi &= ln(C)^\vee\\ \end{split} \end{equation}$$
We provide a deeper perspective on the not unique $\phi$ problem in Section $\ref{sec: append why is not unique}$ appendix.\
A closed form solution for both cases are given by
$$\begin{equation} \begin{split} C &= cos(\phi)I + (1-cos(\phi))aa^T + sin(\phi)a^\wedge\\ \phi &= acos(\frac{tr(C)-1}{2}) + 2\pi m \end{split} \end{equation}$$
where $\mathbf{\phi} = \phi a$. while the unit vector $a$ is the eigen vector, whose eigen value is 1.

Pose SE(3)

Similarly we define the following $T\in SE(3), \xi^\wedge \in \mathfrak{se}(3)$, according to Eq. $\ref{ep:exponentialA}$

$$\begin{equation} \begin{split} T = exp(\xi^\wedge) &= \sum_{n=0}^{\infty} \frac{1}{n!}(\xi^\wedge)^n\\ \xi &= ln(T)^\vee \end{split} \end{equation}$$
We also give a closed form solution for the above equations.
$$\begin{equation} \begin{split} T &= \begin{bmatrix}C,r\\0,1\end{bmatrix}\\ \rho &= J^{-1}r \end{split} \end{equation}$$
where J is the left Jacobian matrix.
$$\begin{equation}\label{eq:EM:Pse:jac} \begin{split} J &= \sum_{n=0}^{\infty} \frac{1}{(n+1)!}(\phi^\wedge)^n\\ r &= J\rho\\ \end{split} \end{equation}$$

Jacobian

The concept of “Jacobian” here is different from the one used in optimization.This Jacobian matrix is defined in Eq. $\ref{eq:EM:Pse:jac}$ and can be used in two sceneries: transforming $\xi$ to $r$ in SE(3), and also this J can be used in derivatives.\
As we defined Jacobian matrix, we can give the closed form solution to it in the same way with SO(3).

$$\begin{equation} J = \frac{sin(\phi)}{\phi}I+(1-\frac{sin(\phi)}{\phi})aa^T+\frac{1-cos(\phi)}{\phi}a^\wedge \end{equation}$$
In the same way, we can also give the expression of $J^{-1}, J^TJ$.

Adjoint

This section can be covered later.

Baker Campbell Hausdorff

For scalar values, we have

$$\begin{equation} exp(a)exp(b) = exp(a+b) \end{equation}$$
BCH can be used to do this job (of course the format is different for Lie groups). After some derivation we have
$$\begin{equation} \begin{split} exp(A+B) = \lim_{a\rightarrow \infty} (exp(A/\alpha)exp(B/\alpha))^\alpha \end{split} \end{equation}$$

Rotation SO(3)

For the special case of SO(3), we have

$$\begin{equation} \begin{split} ln(C_1C_2)^\vee \approx \begin{cases} J(\phi_2)^{-1}\phi_1+\phi_2, &if \phi_1 small\\ \phi_1+J(-\phi_1)^{-1}\phi_2, &if \phi_2 small\\ \end{cases} \end{split} \end{equation}$$

Pose SE(3)

Similarly we can get

$$\begin{equation} \begin{split} ln(T_1T_2)^\vee \approx \begin{cases} \mathfrak{J}(\xi_2)^{-1}\xi_1 +\xi_2, & if \xi_1 small\\ \xi_1 +\mathfrak{J}(-\xi_1)^{-1}\xi_2, &if \xi_2 small \end{cases} \end{split} \end{equation}$$

Distance, volume, Integration

Rotation SO(3)

We define the distance between two SO(3)

$$\begin{equation} \phi_{12} = ln(C_1^TC_2)^\vee \end{equation}$$
Perturbing $\phi$ by a little results in a new Rotation matrix $C'$
$$\begin{equation} ln(C'C^T) \approx ln(exp(J_l\delta \phi)^\wedge CC^T) = J_l\delta\phi \end{equation}$$

Pose SE(3)

The distance

$$\begin{equation} \xi_{12} = ln(T_1^{-1}T_2)^\vee \end{equation}$$
The perturbing
$$\begin{equation} ln(T'T^T)^\vee\approx \mathfrak{J}\delta \xi \end{equation}$$

Calculus and Optimization

Rotation matrix

We can derive that

$$\begin{equation} \frac{\partial Cv}{\partial \phi} = -(Cv)^\wedge J \end{equation}$$
If we have complex function $u(x),with x = Cv$, we have
$$\begin{equation} \frac{\partial u}{\partial \phi} =\frac{\partial u}{\partial x}\frac{\partial x}{\partial \phi} = - \frac{\partial u}{\partial x}(Cv)^\wedge J \end{equation}$$
After some substuting, we have simpler case:
$$\begin{equation} \frac{\partial Cv}{\partial \psi} = -(Cv)^\vee \end{equation}$$
which has no J.

Poses

Let $T\in SE(3)$, $p\in HC$, we can find that:

$$\begin{equation} \begin{split} Tp &= exp(\xi^\wedge)T_{op}p \approx (1+\xi^\wedge)T_{op}p\\ &= T_{op}p + \xi^\wedge T_{op}p= T_{op}p + (T_{op}p)^\odot \xi \end{split} \end{equation}$$
So we can get the derivatives easily:
$$\begin{equation} \frac{\partial Tp}{\partial \xi} = (T_{op}p)^\odot \end{equation}$$

Appendix

why $\phi$ is not unique

Some background knowledge. The first equation is about the properties of skew synmetric matrix. Using this properties, we can simplify the high order on skew matrix to scalar numbers.

$$\begin{equation}\label{eq:Apwhy} \begin{split} w \in \mathbb{R}^3, (w^{\wedge})^3 &= - (w^Tw)w^{\wedge} \\ sin(\theta) &= \frac{1}{1!}\theta + \frac{-1}{3!}\theta^3+...\\ cos(\theta) & = 1+\frac{-1}{2!}\theta^2 + \frac{1}{4!}\theta^4+... \end{split} \end{equation}$$
Let $\mathbf{\phi} = \phi a, \phi \in \mathbb{R}, a\in \mathbb{R}^3, |a|=1$, refer to Eq. $\ref{eq:em:RSo3}$ and Eq. $\ref{eq:Apwhy}$ we can expand the exponential map as
$$\begin{equation} exp(\phi^\wedge) = exp(\phi a^\wedge) = cos(\phi)I + (1-cos(\phi))aa^T + sin(\phi)a^\wedge \end{equation}$$
As $\phi$ is all inside the cos and sin functions, so the exp value will be satified with infinite number of $\phi+2\pi m, m \in uint$. Note that rotation matrix is unique, but the 3-parameter values can not be unique, so there is a singularity problem in 3-parameter rotation matrix.

Determinant of Rotation matrix

Exponential map provide another way to see the determinant of rotation matrix. Here we introduce a theory in general matrix. For any square matrix $A$ with complex elements, we have

$$\begin{equation} det(exp(A)) = exp(tr(A)) \end{equation}$$
In our case

$$\begin{equation} det(C) = det(exp(\phi^\wedge)) = exp(tr(\phi^\wedge)) = exp(0) = 1 \end{equation}$$

Example BA

Using 3D points

We give a simplified BA example here, where the 3D points are known as ground truth, and the pose is to be estimated. let K be the intrinsic matrix, $P^G,P^L$ be the 3D points in global and local frame, $u,v$ be the measured pixel points.

$$\begin{equation} \begin{split} P^L &= R^{-1}(P^G-T) \\ obj &= KP^L/P^L(3) - \begin{bmatrix}u\\v \end{bmatrix} \\ \end{split} \end{equation}$$
Jacobian matrix. Derive on R and t of the objective functions. Let $v=(P^G-T)$, $\delta$ is perturb value on R. We can define the derivative of $P^L$ on variables.
$$\begin{equation} \begin{split} R^{-1}v &= (exp(\delta^\wedge)R_{op})^{-1}v\\ &= R_{op}^{-1}exp(\delta^\wedge)^{-1}v = R_{op}^{-1}exp(-\delta^\wedge)v \\ &\approx R_{op}^{-1}(I-\delta^\wedge)v = R_{op}^{-1}v -R_{op}^{-1}\delta^\wedge v \\ &= R_{op}^{-1}v + R_{op}^{-1}v^\wedge\delta\\ \frac{\partial P^L}{\partial \delta} &= R_{op}^{-1}v^\wedge\\ \frac{\partial P^L}{\partial tx} &= R^{-1}(-[1;0;0]); \end{split} \end{equation}$$
We then define the derivative of objective function f on variables.
$$\begin{equation} \begin{split} f' = K\frac{(P^L)'P^L(3)-P^LP^L(3)'}{(P^L(3))^2} \end{split} \end{equation}$$
Please refer to the code $"run\_so3\_spBA.m"$.

Using homogeneous coordinates

In the above code, we use 3D points to do the BA, which is not convenient. In this part, we will give you a homogeneous coordinates (HC) example. You can refer to the lecture given by Prof. Cyrill Stachniss. The HC can be regarded as another cooridinates with axis and origin.
So we can use SE(3) to represent the transformations

$$\begin{equation} \begin{split} T &= \begin{bmatrix}R,t\\0,1 \end{bmatrix}\\ P^G &= TP^L\\ P^L &= T^{-1}P^G\\ P^I &= Keye(3,4)P^L \\ obj &= P^I - P^I(3)\begin{bmatrix} u\\v\\1\end{bmatrix} \end{split} \end{equation}$$
Jacobian matrix: we can use perturbation to derive derivatives on T, which is very straightforward.
$$\begin{equation} \begin{split} T^{-1}p &= (exp(\xi^\wedge)T_{op})^{-1}p = T_{op}^{-1}exp(\xi^\wedge)^{-1}p =T_{op}^{-1}exp(-\xi^\wedge)p \\ & \approx T_{op}^{-1}(1-\xi^\wedge)p = T_{op}^{-1}p - T_{op}^{-1}\xi^\wedge p \\ & = T_{op}^{-1}p - T_{op}^{-1} p^{\odot}\xi\\ \frac{\partial P^L}{\partial \xi} &= -T_{op}^{-1} p^{\odot}\\ \frac{\partial P^I}{\partial \xi} &= Keye(3,4)\frac{\partial P^L}{\partial \xi} \\ \frac{\partial obj}{\partial \xi} &= \frac{\partial P^I}{\partial \xi} - \frac{\partial P^I}{\partial \xi}(3)\begin{bmatrix} u\\v\\1 \end{bmatrix} \end{split} \end{equation}$$
Amazing!
Comparison: run : $run\_so3\_spBA$, it takes 20 iterations to converge to the optimal values. If we run $run\_se3\_hc$, it takes just 5 iterations to converge. So we can draw a conclusion that, it’s more efficient using Lie Group SE(3) to converge.