# 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)$a = (1,2,3), b = (3,4,5)$ are elements of the vector space, the plus operator can be applied by c=a+b$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$C$ for SO(3) and T$T$ for SE(3). Suppose C1,C2,C3SO(3)$C_1,C_2,C_3\in SO(3)$ and T1,T2,T3SE(3)$T_1,T_2,T_3 \in SE(3)$:

1. C,T are also a differential manifold.
2. C1C2SO(3)$C_1C_2\in SO(3)$ and T1T2SE(3)$T_1T_2\in SE(3)$.
3. Associativity: (C1C2)C3$(C_1C_2)C_3$ = C1(C2C3)$C_1(C_2C_3)$. (T1T2)T3=T1(T2T3)$(T_1T_2)T_3=T_1(T_2T_3)$
4. Identity: C1I=IC1=C1$C_1I=IC_1=C_1$, T1I=IT1=T1$T_1I=IT_1=T_1$
5. Invertibility: C11SO(3)$C_1^{-1}\in SO(3)$, T11SE(3)$T_1^{-1}\in SE(3)$

Necessary and sufficient conditions fro SO(3): CTC=I,det(C)=1$C^TC = I, det(C) =1$, and for SE(3): T=[C,t0,1],where,CSO(3),tR3$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 V$\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]V$[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, so(3),se(3)$\mathfrak{so}(3),\mathfrak{se}(3)$.

## Rotation RO(3)

The Lie algebra associated with SO(3) is given by
1. Vector space: so(3)={Φ=ϕR3×3|ϕR3}$\mathfrak{so}(3) =\{\mathbf{\Phi}=\phi^\wedge\in \mathbb{R}^{3\times 3}|\phi \in \mathbb{R}^3\}$
2. Field: R$\mathbb{R}$
3. Lie bracket:[Φ1,Φ2]=Φ1Φ2Φ2Φ1$[\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, se(3)={Ξ=ξR4×4|ξR6}$\mathfrak{se}(3)=\{\Xi=\xi^\wedge \in \mathbb{R}^{4\times4}|\xi \in \mathbb{R}^6\}$
2. Field: R$\mathbb{R}$
3. Lie bracket: [Ξ1,Ξ2]=Ξ1Ξ2Ξ1Ξ2$[\Xi_1,\Xi_2] =\Xi_1\Xi_2-\Xi_1\Xi_2$
where

ξ=[ρϕ]=[ϕ,ρ0,1]R4×4

# 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:

\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}

## Rotation SO(3)

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

\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}

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
Cϕ=cos(ϕ)I+(1cos(ϕ))aaT+sin(ϕ)a=acos(tr(C)12)+2πm

where ϕ=ϕa$\mathbf{\phi} = \phi a$. while the unit vector a$a$ is the eigen vector, whose eigen value is 1.

## Pose SE(3)

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

T=exp(ξ)ξ=n=01n!(ξ)n=ln(T)

We also give a closed form solution for the above equations.
Tρ=[C,r0,1]=J1r

where J is the left Jacobian matrix.
\label{eq:EM:Pse:jac}
\begin{split}
J &= \sum_{n=0}^{\infty} \frac{1}{(n+1)!}(\phi^\wedge)^n\\
r &= J\rho\\
\end{split}

## 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$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).

J=sin(ϕ)ϕI+(1sin(ϕ)ϕ)aaT+1cos(ϕ)ϕa

In the same way, we can also give the expression of J1,JTJ$J^{-1}, J^TJ$.

This section can be covered later.

# Baker Campbell Hausdorff

For scalar values, we have

exp(a)exp(b)=exp(a+b)

BCH can be used to do this job (of course the format is different for Lie groups). After some derivation we have
exp(A+B)=lima(exp(A/α)exp(B/α))α

## Rotation SO(3)

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

ln(C1C2){J(ϕ2)1ϕ1+ϕ2,ϕ1+J(ϕ1)1ϕ2,ifϕ1smallifϕ2small

## Pose SE(3)

Similarly we can get

ln(T1T2){J(ξ2)1ξ1+ξ2,ξ1+J(ξ1)1ξ2,ifξ1smallifξ2small

# Distance, volume, Integration

## Rotation SO(3)

We define the distance between two SO(3)

ϕ12=ln(CT1C2)

Perturbing ϕ$\phi$ by a little results in a new Rotation matrix C$C'$
ln(CCT)ln(exp(Jlδϕ)CCT)=Jlδϕ

The distance

ξ12=ln(T11T2)

The perturbing
ln(TTT)Jδξ

# Calculus and Optimization

## Rotation matrix

We can derive that

Cvϕ=(Cv)J

If we have complex function u(x),withx=Cv$u(x),with x = Cv$, we have
uϕ=uxxϕ=ux(Cv)J

After some substuting, we have simpler case:
Cvψ=(Cv)

which has no J.

## Poses

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

Tp=exp(ξ)Topp(1+ξ)Topp=Topp+ξTopp=Topp+(Topp)ξ

So we can get the derivatives easily:
Tpξ=(Topp)

# 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.

\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}

Let ϕ=ϕa,ϕR,aR3,|a|=1$\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
exp(ϕ)=exp(ϕa)=cos(ϕ)I+(1cos(ϕ))aaT+sin(ϕ)a

As ϕ$\phi$ is all inside the cos and sin functions, so the exp value will be satified with infinite number of ϕ+2πm,muint$\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$A$ with complex elements, we have

det(exp(A))=exp(tr(A))

In our case
det(C)=det(exp(ϕ))=exp(tr(ϕ))=exp(0)=1

# 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, PG,PL$P^G,P^L$ be the 3D points in global and local frame, u,v$u,v$ be the measured pixel points.

PLobj=R1(PGT)=KPL/PL(3)[uv]

Jacobian matrix. Derive on R and t of the objective functions. Let v=(PGT)$v=(P^G-T)$, δ$\delta$ is perturb value on R. We can define the derivative of PL$P^L$ on variables.
R1vPLδPLtx=(exp(δ)Rop)1v=R1opexp(δ)1v=R1opexp(δ)vR1op(Iδ)v=R1opvR1opδv=R1opv+R1opvδ=R1opv=R1([1;0;0]);

We then define the derivative of objective function f on variables.
f=K(PL)PL(3)PLPL(3)(PL(3))2

Please refer to the code "run_so3_spBA.m"$"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

TPGPLPIobj=[R,t0,1]=TPL=T1PG=Keye(3,4)PL=PIPI(3)uv1

Jacobian matrix: we can use perturbation to derive derivatives on T, which is very straightforward.
T1pPLξPIξobjξ=(exp(ξ)Top)1p=T1opexp(ξ)1p=T1opexp(ξ)pT1op(1ξ)p=T1oppT1opξp=T1oppT1oppξ=T1opp=Keye(3,4)PLξ=PIξPIξ(3)uv1

Amazing!
Comparison: run : run_so3_spBA$run\_so3\_spBA$, it takes 20 iterations to converge to the optimal values. If we run run_se3_hc$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.