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)
- SO(3): special orthogonal group.
- 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
- C,T are also a differential manifold.
- C1C2∈SO(3) and T1T2∈SE(3) .
- Associativity: (C1C2)C3 = C1(C2C3) . (T1T2)T3=T1(T2T3)
- Identity: C1I=IC1=C1 , T1I=IT1=T1
- Invertibility: C−11∈SO(3) , T−11∈SE(3)
Necessary and sufficient conditions fro SO(3): CTC=I,det(C)=1 , and for SE(3): T=[C,t0,1],where,C∈SO(3),t∈R3
Lie Algebras
Every Lie group associates with a Lie Algebras, which is in vector space V . More specifically, the Lie Algebras is skew matrix (a square real matrix (subspace) of vector space.), and with some operators Lie brackets.
- Closure: [X,Y]∈V
- Bilinearity: [aX+bY,Z] = a[X,Z] +b[Y,Z]
- Alternating: [X,X] = 0
- 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) .
Rotation RO(3)
The Lie algebra associated with SO(3) is given by
1. Vector space:
so(3)={Φ=ϕ∧∈R3×3|ϕ∈R3}
2. Field:
R
3. Lie bracket:
[Φ1,Φ2]=Φ1Φ2−Φ2Φ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}
2. Field:
R
3. Lie bracket:
[Ξ1,Ξ2]=Ξ1Ξ2−Ξ1Ξ2
where
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{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∈SO(3),ϕ∧∈so(3)
, according to Eq.
???
\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 ϕ problem in Section ??? appendix.\
A closed form solution for both cases are given by
where ϕ=ϕa . while the unit vector a is the eigen vector, whose eigen value is 1.
Pose SE(3)
Similarly we define the following
We also give a closed form solution for the above equations.
where J is the left Jacobian matrix.
\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.
???
and can be used in two sceneries: transforming
ξ
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).
In the same way, we can also give the expression of J−1,JTJ .
Adjoint
This section can be covered later.
Baker Campbell Hausdorff
For scalar values, we have
BCH can be used to do this job (of course the format is different for Lie groups). After some derivation we have
Rotation SO(3)
For the special case of SO(3), we have
Pose SE(3)
Similarly we can get
Distance, volume, Integration
Rotation SO(3)
We define the distance between two SO(3)
Perturbing ϕ by a little results in a new Rotation matrix C′
Pose SE(3)
The distance
The perturbing
Calculus and Optimization
Rotation matrix
We can derive that
If we have complex function u(x),withx=Cv , we have
After some substuting, we have simpler case:
which has no J.
Poses
Let
T∈SE(3)
,
p∈HC
, we can find that:
So we can get the derivatives easily:
Appendix
why ϕ 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{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 ϕ=ϕa,ϕ∈R,a∈R3,|a|=1 , refer to Eq. ??? and Eq. ??? we can expand the exponential map as
As ϕ is all inside the cos and sin functions, so the exp value will be satified with infinite number of ϕ+2πm,m∈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
In our case
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
be the 3D points in global and local frame,
u,v
be the measured pixel points.
Jacobian matrix. Derive on R and t of the objective functions. Let v=(PG−T) , δ is perturb value on R. We can define the derivative of PL on variables.
We then define the derivative of objective function f on variables.
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
Jacobian matrix: we can use perturbation to derive derivatives on T, which is very straightforward.
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.