SLAMBOOK Chapter3: Lie Group and Lie Algebra

In Chapter2: we introduced the coordinate transformation and 3d rigid body motion which are the basics to do SLAM. However in SLAM, the pose and maps are unknown and need to estimate and optimize minimize the error. When solving an optimization problem using the form of a matrix is difficult, Lie Group and Lie Algebra are introduced to simplify the solution.

 

Group

The rotation matrix and transformation matrix is well known form to express the rotation and the movement in the 3d space. The rotation matrix constitutes the special orthogonal group \(\mathrm{SO}(3)\), and the transformation matrix constitutes the special Euclidean group \(SE(3)\) as:

 

$$ \mathrm{SO(3)} = \{\mathbf{R} \in \mathbb{R}^{3 \times 3} \mid \mathbf{RR}^{T}=\mathbf{I}, \mathrm{det}(\mathbf{R}) =1 \} $$

$$ \mathrm{SE}(3) = \begin{Bmatrix}
\mathbf{T}= \begin{bmatrix}
 \mathbf{R}& t\\
0 &  1\\
\end{bmatrix} \in \mathbb{R}^{4 \times 4} \mid \mathbf{R} \in \mathrm{SO}(3), t \in \mathbb{R}^3 \end{Bmatrix}
 $$

 

So what is a Group? A group is an algebraic structure of one set plus one operator, denoted as \(G = (A,\cdot)\). \(G\) is a group if the operation satisfies the following conditions:

Properties of Group

For better understanding, General Linear group \(\mathrm{GL(n)}\) is a common group in the matrix. The invertable matrix of \(n \times n\) with matrix multiplication. If we add conditions that the matrix is orthogonal and the determinant should be 1, we can get the special orthogonal group \(\mathrm{SO(n)}\). The groups that are "continuous" and "smooth" are called Lie Groups.

 

Introduction of the Lie Algebra

Conceptual relationship of a Lie group and the corresponding Lie algebra

In SLAM, the pose of a camera changes continuously over time. We can write the rotation as a function of time: \(\mathbf{R}(t)\). Since the rotation matrix is orthogonal, we have:

$$ \begin{equation} \mathbf{R}(t)\mathbf{R}(t)^T = \mathbf{I} \end{equation}$$

Differentiate both sides of equation, then:

$$ \begin{equation} \mathbf{\dot{R}}(t)\mathbf{R}(t)^{T} + \mathbf{R}(t)\mathbf{\dot{R}}(t)^{T} = 0 \\ \mathbf{\dot{R}}(t)\mathbf{R}(t)^{T} = -(\mathbf{\dot{R}}(t)\mathbf{R}(t)^{T})^{T}\end{equation}$$

Since the \(\mathbf{\dot{R}}(t)\mathbf{R}(t)^{T}\) is skew-symmetric matrix, there's a corresponding vector \(\phi(t) \in \mathbb{R}^3\).

$$ \begin{equation} \mathbf{\dot{R}}(t)\mathbf{R}(t)^{T} = \phi (t)^{\wedge} \end{equation} $$

$$ \begin{equation} \mathbf{\dot{R}}(t) = \phi(t)^{\wedge}\mathbf{R}(t)\end{equation} $$

From the first-order Taylor expansion around \(t = t_{0}\), we can write as:

$$  \begin{equation} \mathbf{R}(t) \approx \mathbf{R}(t_{0}) + \mathbf{\dot{R}}(t_{0})(t-t_{0})\end{equation} $$

 

Considering that the rotation matrix \(\mathbf{R}(0) = \mathbf{I}\) and  by equation (4), 

$$ \begin{equation} \mathbf{R}(t) = \mathbf{I} + \phi(t_{0})^{\wedge}(t) \end{equation}$$

 

\(\phi \) reflects the derivative of \(\mathbf{R}\), an called the tangent space of \(\mathrm{SO}(3)\) as shown in the figure.

 

Lie Algebra \(\mathfrak{so}(3)\) and  \(\mathfrak{se}(3)\)

The \(\phi \) mentioned above is actually a Lie algebra corresponding to \(\mathrm{SO}(3)\). Each Lie group has a Lie algebra corresponding to it. A Lie algebra consists of a set \(\mathbb{V}\), a scalar field \(\mathbb{F}\), and a binary operation \([,]\) called Lie bracket. The Lie algebra \((\mathbb{V},\mathbb{F}, [,])\) satisfies the following properties.

Properties of Lie Algebra

$$ \begin{equation} \mathfrak{so}(3) = \{\phi \in \mathbb{R}^{3} \,\mathrm{or}\, \Phi = \phi^{\wedge} \in \mathbb{R}^{3 \times 3}\} \end{equation} $$

Under the definition of \(\mathfrak{so}(3)\) as equation (7) the Lie bracket is:

$$ [\phi_{1}, \phi_{2}] = (\Phi_{1}\Phi_{2} - \Phi_{2}\Phi_{1})^{\vee} $$ 

where the operator is matrix multiplication.

 

Similar to \(\mathfrak{so}(3)\), \(\mathfrak{se}(3)\) is located in the \(\mathbb{R}^{6}\) space:

$$ \begin{equation}\mathfrak{se}(3) = \begin{Bmatrix}
\xi = \begin{bmatrix}
\rho \\ \phi 
\end{bmatrix} \in \mathbb{R}^{6}, \rho \in \mathbb{R}^{3}, \phi \in \mathfrak{so}(3), \xi^{\wedge} = \begin{bmatrix}
\phi^{\wedge} & \rho  \\
0 & 0 \\
\end{bmatrix} \in \mathbb{R}^{4 \times 4}
\end{Bmatrix}\end{equation} $$

The Lie bracket for \(\mathfrak{se}(3)\) is:

$$ \begin{equation}[\xi_{1}, \xi_{2}] = (\xi^{\wedge}_{1}\xi^{\wedge}_{2} - \xi^{\wedge}_{2}\xi^{\wedge}_{1}) ^{\vee}\end{equation}$$

 

---

Exponetial and Logarithmic mapping

The correspondence between Lie Group and Lie Algebra

So far we've looked at what a Lie algebra is, but what really matters is the relationship between the Lie group and Lie algebra, and why Lie algebra is used.

 

As shown in the above figure,  the relationship between \(\mathrm{SO}(3)\) and \(\mathfrak{so}(3)\), \(\mathrm{SE}(3)\) and \(\mathfrak{se}(3)\) is given by the exponential mapping and Logarithmic mapping.

The exponential of an arbitrary matrix can be written as:

$$ \begin{equation} \mathrm{exp}(\mathbf{A}) = \sum_{n=0}^{\infty}\frac{1}{n!}\mathbf{A}^n \end{equation}$$

 

If we put \(\phi^{\wedge} \) instead of \(\mathbf{A} \), it's the conversion from \(\mathfrak{so}(3)\) to \(\mathrm{SO}(3)\). However it's impossible to calculate the infinite sum.

 

Recall \(\phi ^{\wedge}\) is skew-symmetric matrix, and let \(\phi = \theta\mathbf{n}\)  where \(\lvert n\rvert = 1\). Then we can derive two properties:

$$\begin{equation} \mathbf{n}^{\wedge}\mathbf{n}^{\wedge} = \begin{bmatrix}
-n^{2}_{2} - n^{2}_{3} & n_{1}n{2} & n_{1}n{3} \\
n_{1}n{2} & -n^{2}_{1} - n^{2}_{3}  & n_{2}n{3} \\
n_{1}n{3} & n_{2}n{3} & -n^{2}_{1} - n^{2}_{2}  \\
\end{bmatrix} = \mathbf{n}\mathbf{n}^{T} - \mathbf{I}\end{equation} $$

$$\begin{equation}\mathbf{n}^{\wedge}\mathbf{n}^{\wedge}\mathbf{n}^{\wedge} = \mathbf{n}^{\wedge}(\mathbf{n}\mathbf{n}^{T} - \mathbf{I}) =-\mathbf{n}^{\wedge}  \end{equation} $$

 

Now we can rewrite the infinite sum to a familiar equation as:

$$\begin{equation} \begin{aligned} &\mathbf{exp}(\theta\mathbf{n}^{\wedge}) = \sum_{n=0}^{\infty}\frac{1}{n!}(\mathbf{\theta\mathbf{n}})^n \\&
= \mathbf{I}  + \theta\mathbf{n}^{\wedge} + \frac{1}{2!}\theta^{2}\mathbf{n}^{\wedge}\mathbf{n}^{\wedge} + \cdots \\&
= \mathbf{n}\mathbf{n}^{T}  + (\theta - \frac{1}{3!}\theta^{3} + \frac{1}{5!}\theta^{5})\mathbf{n}^{\wedge} - (1 - \frac{1}{2!}\theta^{2} + \frac{1}{4!}\theta^{4})\mathbf{n}^{\wedge}\mathbf{n}^{\wedge} \\&
 =  \mathbf{n}^{\wedge}\mathbf{n}^{\wedge} + \mathbf{I} + \sin\theta\mathbf{n}^{\wedge} - \cos\theta\mathbf{n}^{\wedge}\mathbf{n}^{\wedge} \\&
= \cos\theta\mathbf{I}  + (1-\cos\theta)\mathbf{n}\mathbf{n}^{T} + \sin\theta\mathbf{n}^{\wedge}\end{aligned}
\end{equation} $$

 

In inverse, we can also map the elements in \(\mathrm{SO}(3)\) to  \(\mathfrak{se}(3)\):

$$\begin{equation}\phi = \ln(\mathbf{R})^{\vee} = (\sum_{n=0}^{\infty}\frac{(-1)^{n}}{n+1}(\mathbf{R}-\mathbf{I}))^{\vee} \end{equation} $$

 

It is not really important to derive formulas through these complex processes, but we now know the mapping between Lie groups and Lie algebra.

 

---

Lie Algebra Derivation and Perturbation Model

So far, this long complex process was for to use Lie algebra for the optimization. The rotation using the matrix form of \(\mathrm{SO}(3)\) is done by matrix multiplication. For example, suppose we need to solve an optimization problem:

$$\begin{equation}\min_{T}{J}(\mathbf{T}) = \sum_{i=1}^{N}\lvert\lvert \mathbf{z}_i - \mathbf{Tp}_i\rvert\rvert_{2}^{2} \end{equation} $$

To solve such an optimization problem, we need to calculate the derivative of \(J\) by \(\mathbf(T)\). However, since \(\mathrm{SO}(3)\) and \(\mathrm{SE}(3)\) have no well-defined addition, so the derivatives cannot be defined in their common form. So what we want is to derive addition of Lie algebra from multiplcation of 2 Lie Groups as:

$$ \begin{equation} \ln{(\mathrm{exp(\mathbf{\phi}_{1}^{\wedge})\mathrm{exp(\mathbf{\phi}_{2}^{\wedge}})}}) = \phi_{1}^{\wedge} + \phi_{2}^{\wedge} \end{equation}$$

 

Unfortunatly, this is not the case in the matrix. The BCH(Baker-Campbell-Hausdorff) formula gives the complete form as:

$$ \begin{equation} \ln{(\mathrm{exp}(\mathbf{A})\mathrm{exp}(\mathbf{B}))} = \mathbf{A} +\mathbf{B} + \frac{1}{2}[ \mathbf{A},  \mathbf{B}] + \frac{1}{12}[ \mathbf{A},[ \mathbf{A}, \mathbf{B}]] + \cdots \end{equation} $$

 

Of course, we will not deal with the mathematical proof, but BCH formula gives the following approximation:

 

$$ \begin{equation}
\ln(\mathrm{exp}(\phi_{1}^{\wedge})\,\mathrm{exp}(\phi_{2}^{\wedge}))^{\vee}  
\approx \left\{
\begin{matrix}
\mathbf{J}_{l}(\phi_{2})^{-1}\phi_{1} + \phi_{2} \quad \text{when } \phi_{1} \text{ is a small amount} \\ 
\mathbf{J}_{r}(\phi_{1})^{-1}\phi_{2} + \phi_{1} \quad \text{when } \phi_{2} \text{ is a small amount}
\end{matrix}
\right.
\end{equation}$$

 

Similarly, for \(\mathrm{SE}(3)\), there is a similar BCH approximation:

Finally, we have all building blocks to solve the problem of derivation using Lie Algebra. There are two approaches.

Derivative Model

Suppose we rotate point \(\mathbf{p}\) and get \(\mathbf{Rp}\). Since \(\mathbf{SO}(3)\) has no addition operator, we will use Lie algebra instead:

Thus, we derived the derivative ot the rotated point in Lie algebra. However, \(\mathbf{J}_{l}\) is still to complicated to calculate it. For the simpler derivative, the perturbation model is introduced to compute derivatives

 

Perturbation model

Another way is to perturb \(\mathbf{R}\) by \(\delta \mathbf{R}\) as shown in above equations. \(\varphi\) is the Lie algebra of \(\delta \mathbf{R}\). In the perturbation model, Jacobian \(\mathbf{J_{l}}\) is omitted compared to the derivative model, which makes it more practical. Similarly Derivative of \(mathrm{SE}(3)\) can me calculated as:

 

---

Conclusion

We've got through too much formulars and proof, but the important thing is to understand why the Lie algebra is used and what it means in SLAM. There's no reason to be scared by the mathematics written to help you understand it. Lie algebra will be covered again in later chapter, solving actual optimization problem.