# Lie Groups and Lie Algebra

## Motivation

In robotic vision, we frequently deal with problems where the motion of a camera is to be estimated. For instance, if a drone is being moved around in a scene and we were to reconstruct a 3D model of the scene using the images from the drone’s camera, we need a way to model and represent the trajectory of the camera and know where the images were taken from, before we can recover the geometry of the scene. The motion of a camera is typically modeled as a rigid body motion (simply a rotation and a translation, more formally called a Special Euclidean Transformation in $$SE(3)$$), a topic which has been studied extensively.

A rigid body motion can be represented in multiple ways and our choice of representation is closely tied to the application. For instance, the 3D + YPR representation is a popular choice in aerial robotics but not so much outside of it because of its famous gimbal lock problem.

The following is a more widely used representation, called the homogeneous matrix representation, that allows us to represent motion as a linear transformation.

$T = \left[ \begin{array}{ccc|c} & & & x\\ & R & & y\\ & & & z\\ \hline 0 & 0 & 0 & 1 \end{array} \right]$

where $$R$$ is a 3 x 3 rotation matrix (more formally called a Special Orthogonal Transformation in $$SO(3)$$)

$R = \left[ \begin{array}{ccc} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{21} & r_{23} \\ r_{31} & r_{32} & r_{33} \\ \end{array} \right]$

However, such a representation is actually a bit ‘nasty’ to work with. The rotation matrix $$R$$ is a part of a non-linear space, and even though rotations only present three degrees of freedom, there are nine parameters for rotation in this representation, constrained by $$R^\top(t)R = I$$ and det($$R$$) = +1. So if we were to estimate the motion of a camera over multiple images, we’ll have to estimate nine parameters for the rotation alone and that would have to be solved for as a constrained optimization problem which can be very tedious to implement.

The theory of Lie groups and their corresponding algebra presents us with a much nicer lower-dimensional linear representation for rigid body motion, one which is increasingly being used in computer vision these days. In this post I’ll discuss about the conceptual model of Lie groups and Lie algebra, the rough knowledge that is needed in order to use it effectively in computer vision problems.

## Lie Parameterization of Rotations

Consider a rotational trajectory $$R(t) : \mathbb{R} \rightarrow SO(3)$$ representing the rotational motion of a camera, where SO(3) is the set of all rotation matrices.

$SO(3) \stackrel{.}{=} \left\{ R \in \mathbb{R}^{3x3} \;|\; R^{T}R = I, det(R) = +1 \right\}$

We know by definition,

$$R(t)R(t)^{\top} = I$$ Computing its derivative with respect to time $$t$$, we get $$\dot{R}(t)R^\top(t) + R(t)\smash{\dot{R}}^\top(t) = 0$$ $$\Rightarrow\dot{R}(t)R^\top(t) = -(\dot{R}(t)R^\top(t))^\top$$ The resulting equation shows that $$\dot{R}(t)R^\top(t)$$ is a skew-symmetric matrix, which implies that there must exist a vector $$w(t)\in \mathbb{R}^3$$ such that $$\dot{R}(t)R^\top(t) = \hat{\omega}(t) \Leftrightarrow\dot{R}(t) = \omega(t)R(t)$$ Now if $$R(t_0) = I$$, we have $$\dot{R}(t_0) = \hat{\omega}(t_0)$$ and so a Taylor series approximation of $$R(t)$$ about $$t = t_0$$ would result in $$R(t_o + dt) \approx I_o + \hat{\omega}(t_o)dt$$

What this means is that infinitesimal rotations around the identity are locally only dependent on the three dimensional $$(\omega_1,\omega_2,\omega_3)$$ space of skew symmetric matrices, also called the tangent space. This tangent space at the identity is more formally defined as the Lie algebra $$so(3)$$ of the Lie group $$SO(3)$$.

$$so(3) \stackrel{.}{=} \{ \hat{\omega} \in \mathbb{R}^{3 x 3} \;|\; \omega \in \mathbb{R}^{3}\}$$

This tangent space is a linear space with three degrees of freedom and no additional constraints which makes it much easier to work with than its corresponding lie group for motion estimation and optimization problems. This allows us to model rotations as skew-symmetric matrices with three free parameters.

## The Exponential Map

We shall now try to formulate the mapping that will allow us to represent elements of the lie group $$SO(3)$$ in terms of its lie algebra $$so(3)$$.

From the above discussion we know,

$\dot{R}(t) = \hat{\omega}R(t)\,,\,R(t_o) = I$

This is a linear ordinary differential equation with inital state at $$I$$, whose solution is given by

$\boxed{R(t) = e^{\hat{\omega}t}}$

where $$e^{\hat{\omega}t}$$ is the matrix exponential

$e^{\hat{\omega}t} = I + \hat{\omega}t + \frac{(\hat{\omega}t)^2}{2!} + \frac{(\hat{\omega}t)^3}{3!} + …$

A useful interpretation of this solution is that of a rotation around the axis $$\omega \in \mathbb{R}^3$$ by an angle of $$t$$ radians.

Thus, we have an exponential map that allows us take an element from the Lie algebra so(3) to its corresponding element in the Lie group SO(3). $exp: so(3) \rightarrow SO(3); \; \hat{\omega} \mapsto e^{\hat{\omega}}$

A closed-form expression for actually computing the exponential map is given by the Rodrigues’ formula,

$\boxed{ e^{\hat{\omega}} = I + \frac{\hat{\omega}}{\Vert{\omega}\Vert}\sin{\Vert{\omega}\Vert} + \frac{\hat{\omega}^2}{\Vert{\omega}\Vert^2}(1 - \cos(\Vert\omega\Vert)) }$

We have an inverse to this exponential map, given by the logarithm, that’ll allow us to take an element from the Lie group to the Lie algebra. In this case, a rotation matrix to its skew symmetric matrix. If $$R \neq I$$, then

$\boxed{\Vert{\omega}\Vert = \cos^{-1}\left(\frac{trace(R) - 1}{2}\right), \; \frac{\omega}{\Vert{\omega}\Vert} = \frac{1}{2\sin{(\Vert{\omega}\Vert)}} \left[ \begin{array}{c} r_{32} - r_{23}\\ r_{13} - r_{31}\\ r_{21} - r_{12} \end{array} \right]}$

If $$R = I$$ then, $$\Vert{\omega}\Vert = 0$$ and the axis is not defined.
However this representation is not unique since increasing the angle by multiples of $$2\pi$$ will give the same $$R$$.

## Lie Parameterization of Rigid Body Motion

The ideas that were developed for rotations can be extended to rigid body motion as well. The set of rigid body motions SE(3) has a corresponding Lie algebra se(3), defined as the set of 4 x 4 ‘twist’ matrices $$\hat{\xi}$$

$SE(3) \stackrel{.}{=} \left\{ g = \begin{pmatrix} R & t\\ 0 & 1\\ \end{pmatrix} \;|\; R \in SO(3), t \in \mathbb{R}^{3} \right\} \subset \mathbb{R}^{4x4}$
$se(3) \stackrel{.}{=} \left\{ \hat{\xi} = \begin{pmatrix} \hat{\omega} & v\\ 0 & 0\\ \end{pmatrix} \;|\; \hat{w} \in so(3), v \in \mathbb{R}^{3} \right\} \subset \mathbb{R}^{4x4}$

The twist matrix $$\hat{\xi}$$ which consists of a rotational skew-symmetric matrix and a 3D vector, is a tangent vector along some $$g$$. $$\hat{\xi}$$ can further be simplied to a 6-vector $$\xi$$, called the twist coordinates, which is defined as,

$\xi = \begin{pmatrix} v\\ \omega\\ \end{pmatrix} \in \mathbb{R}^6$

$$v$$ is called the linear velocity vector, which is related to the translation vector $$t$$ as follows,

${(I - e^{\hat{\omega}})\hat{\omega}v + \omega\omega^{T}v \over \Vert\omega\Vert} = t$

Like we saw in the case of rotations, rigid body transformations can be similarly modeled using its Lie algebra elements $$\hat{\xi}$$, and $$\xi$$.

## Matlab Code

#### Conversion from se(3) to SE(3)

function [ksi] = computese3(T)
R = T(1:3,1:3);
thetha = acos((trace(R) - 1)/2);

% Handling zero rotation
if(thetha == 0)
w = [0;0;0];
v = T(1:3,4);
ksi = [v w];
else
w = thetha * (1/(2*sin(thetha))*[R(3,2) - R(2,3);R(1,3) - R(3,1);R(2,1) - R(1,2)]);
wx = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0;];
t = T(1:3,4);
% Computing the linear velocity 3 x 1 vector (expression from ethan eade doc)
v = (eye(3) - (1/2 * (wx)) + (((1/(thetha * thetha)) * (1 - ((thetha * sin(thetha)) / (2*(1 - cos(thetha)))))) * (wx * wx)))*t;
ksi = [(v)' w'];
end
end


#### Conversion from SE(3) to se(3)

function [T] = computeSE3(twistVec)
% Splitting the twist vector into angular and translational velocity
% vectors
v = twistVec(1:3)';
w = twistVec(4:6)';

% Constructing the rotational exponential coordinates matrix
wSkewed = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0;];

% Handing zero rotation
if(1 && ~any(w))
R = eye(3);
t = v;
else
% Rodrigues formula
R = eye(3) + (wSkewed/norm(w)) * sin(norm(w)) + (((wSkewed)*(wSkewed))/(norm(w)*norm(w))) * (1 - cos(norm(w)));

% Computing the 3 x 1 translational vector (expression from ethan eade doc)
t = (eye(3) + (((1 - cos(norm(w))) / (norm(w)^2)) * wSkewed) + (((norm(w) - sin(norm(w))) / (norm(w)^3)) * (wSkewed * wSkewed))) * v;
end

% Constructing the 4 x 4 transformation matrix
T = zeros(4);
T(4,4) = 1;

T(1:3,1:3) = R;
T(1:3,4) = t';

end