Lie Theory in Robotics

Nonmenclature

• Special Orthogonal Group (SO):
  • det = 1
  • $R^{-1} = R^T$
• Special Euclidean Group (SE):
  • for SE(3) it’s a $4\times 4$ matrix, namely the transformation matrix

Introduction: Least Square Method in Robotics

For pose control and estimation, usually we have a linear form $f: \mathbb R^k \to \mathbb R$ to evaluate the target value and minimize that value.
The most difficult step is to find a $\Delta x$, s.t. $||f(x+\Delta x) - f(x) ||^2 < ||f(x)||^2$ until the $\Delta x$ is too small.
For example, in the camera coordinate, we have measured pose $p$, and the robot has pose SE $T$, and the measured pose in world coordinate $z$:

$z = Tp$

For measurement noise $w$, we have $z = Tp + w$, and in consideration of measure error, we have $e = z - Tp$, and clearly we need to minimize the error $e$ or say minimize the displacement error $||e||_2$, and what we can modify is the transformation matrix $T$.
By the method of estimation in statistics, the formula would be like the following:

$\arg\min_T J(T) = \arg\min_T\sum_{i=1}^N ||z_i - Tp_i||_2^2$

Like the method of maximum likelihood estimation, we should calculate the derivative of the function, and clearly the rotation matrix $R$ is not closed on the $(R, +)$ group space, thus it’s our purpose to find a better method, which we will introduced as Lie Theory.

Definition: Lie Algebra

For simple SO(3), the rotation matrix, it holds the invariant:

$R(t) R^T(t) = I$

By differentiate both sides, it becomes

$\dot R(t) R^T(t) + R(t)\dot R^T(t) = 0$

$\dot R(t)R^T(t) = -(\dot R(t)R^T(t))^T$

which means that the $\dot R(t) R^T(t)$ is a anti-symmetric matrix, which thus could be expressed in the form of $\phi(t)^{\wedge}$. And then we right multiply a $R(t)$ to both sides below, and obtain:

$\dot R(t)R^T(t) = \phi(t)^\wedge \Leftrightarrow \dot R(t) = \phi(t)^\wedge R(t)$

Namely for any $R(t)\in SO$, its derivative is equal to left multiply a anti-symmetric matrix $\phi(t)^\wedge$
Then we do Taylor Expansion (1 level) near time $t_0=0$ and get

$R(t) = R(t_0) + \dot R(t_0) (t - t_0)$

$R(t) = I + \phi(t_0)^\wedge (t)$

and we have that $\dot R(t) = \phi(t)^\wedge R(t)$, where near $t_0$ the $\phi(t)$ is always the same as $\phi(t_0)\approx \phi_0^\wedge$, with initial value problem $R(0) = I$.
This is a typical linear ODE with solution

$R(t) = e^{\phi_0^\wedge t}$

Since $\phi^\wedge$ reflect the property of $R(t)$, thus it’s in the $R(t)$'s tangent space near $t$

Operation Method

Here we introduce the Lie Brackets $[,]:\mathbb R^3\times \mathbb R^3 \to \mathbb R^3$, where $\Phi := \phi^\wedge$ is the matrix form of the rotation vector $\phi$, and $\vee: SO(3)\to \mathbb R^3$ is the reverse process of $(\cdot) ^\wedge$

$[\phi_1, \phi_2] = (\Phi_1\Phi_2 - \Phi_2\Phi_1)^\vee$

In $SO(3)$, the $(\Phi_1\Phi_2 - \Phi_2\Phi_1)^\vee = \phi_1\times \phi_2$, thus the lie operator is the same as cross product in $SO(3)$ (note here $\Phi_1\phi_2 = \phi_1\times \phi_2\neq (\Phi_1 \Phi_2)^\vee$)
Here are some other properties:

• $[x,x] = 0$
• $[x,[y,z]]+ [y,[z,x]] + [z,[x,y]] = 0$
  And the Lie’s definition for $\mathfrak {so}$ is like $\mathfrak {so}(3) := \{\phi\in \mathbb R^3| \Phi = \phi^\wedge \in \mathbb R^{3\times3}\}$

Lie definition for $\mathfrak {se}(3)$

Similarly as following:

$\mathfrak{se}(3) = \{\xi:= \begin{bmatrix}p\\\theta\end{bmatrix}\in \mathbb R^6,\xi^\wedge :=\begin{bmatrix}\phi^\wedge & p \\ 0 & 0 \end{bmatrix}\in \mathbb R^{4\times 4}\}$

where $\wedge: \mathbb R^6\to \mathbb R^{4\times 4}$