Lie Group and Lie Algebra
Rotation matrices does not form a valid vector space as they are not closed under addition. Instead, they form a structure called Lie group. Specifically, rotation matrices form the Special Orthogonal Group \(R\in SO(3)\):
\[
SO(3) = \{R\in \mathbb{R}^{3\times 3}| RR^\top = I, \det(R) = 1\}
\]
The transformation matrices form the Special Euclidean Group \(SE(3)\):
\[
SE(3) = \left\{T = \left[\begin{array}{cc}R& t\\ 0^\top &1\end{array}\right]\in \mathbb{R}^{4\times 4}| R\in SO(3), t\in\mathbb{R}^3\right\}
\]
Basics
Group
A group contains a set of elements \(G\), a binary operation \(\\bullet\) that satisfy:
\[
\begin{aligned}
&\textbf{Closure:}\quad a, b \in G \implies a \bullet b \in G\\
&\textbf{Associativity:}\quad a, b, c \in G \implies (a \bullet b) \bullet c = a \bullet (b \bullet c)\\
&\textbf{Identity:}\quad \exists I \in G, \textrm{s.t.} \forall a \in G, a \bullet I = I \bullet a = a\\
&\textbf{Inverse:}\quad \forall a \in G, \exists a^{-1} \in G, \textrm{s.t.} a \bullet a^{-1}= I
\end{aligned}
\]
Exponential coordinates of rotations (Lie algebra)
Consider a family of rotation matrices \(R(t)\) that continously transform a point from its original location \((R(0)=I)\) to a different one. Since \(R(t)R(t)^\top = I\), we have:
\[
\frac{d}{dt}(R(t)R(t)^\top)=(\dot R(t)R(t)^\top + R(t) \dot R(t)^\top = 0
\]
This implies:
\[
\dot R(t)R(t)^\top = -(\dot R(t)R(t)^\top)^\top
\]
Thus, we know \(\dot R(t)R(t)^\top\) is a skew-symmetric matrix. This implies that there exists a vector \(\phi(t)^ \wedge\in\mathbb{R}^3\) such that
\[
\dot R(t)R(t)^\top = \phi(t)^ \wedge = \left[\begin{array}{ccc}0 &-\phi_3 & \phi_2\\ \phi_3&0 & -\phi_1\\ -\phi_2&\phi_1 & 0\end{array} \right]\Leftrightarrow \dot R(t) = \phi(t)^ \wedge R(t)
\]
Since \(R(0)=I\), it follows that \(\dot R(0)= \phi(0)^ \wedge\). Thus, the skew-synmmetric matrix \(\phi(0)^ \wedge\in so(3)\) gives the first order approximation of a rotation:
\[
R(dt) = R(0) + dR = I + \phi(0)^ \wedge dt
\]
Lie group and lie algebra
The rotation group \(SO(3)\) is called a Lie group and the space \(so(3)\) is called its Lie algebra. The Lie algebra is the tangent space at the identity of the rotation group \(SO(3)\).
Exponential Map
Let us assume \(\phi^\wedge\) is constant in time. The differential equation system:
\[
\begin{cases}
\dot R(t) = \phi^\wedge R(t)\\
R(0)=I\\
\end{cases}
\]
has the solution
\[
R(t)=\exp(\phi^\wedge t)=\sum_{n=0}^{\infty} \frac{(\phi^\wedge t)^n}{n !}=I+\phi^\wedge t+\frac{(\phi^\wedge t)^2}{2 !}+\ldots
\]
which is rotation around the axis \(w\in\mathbb{R}^3\) by an angle of \(t\) (if |w|=1). This matrix exponential therefore defines a map from the Lie algebra to the Lie group:
\[
exp: \mathfrak{so}(3) \rightarrow \mathrm{SO}(3); \quad \phi^\wedge \rightarrow \exp(\phi^\wedge)
\]
so(3) -> SO(3): The exponential map
Let us define \(\boldsymbol{\Phi} = \phi^\wedge \in \mathbb{R}^{3\times 3}\). Since the vector \(\boldsymbol{\Phi}\) is one-to-one with the skew-symmetric matrix, we say the elements of \(\mathfrak{so}(3)\) are three-dimensional vectors or three-dimensional skew-symmetric matrices:
\[
\mathfrak{so}(3) = \{\phi\in\mathbb{R}^3|\boldsymbol{\Phi}= \phi^\wedge\in\mathbb{R}^{3\times 3}\}
\]
The relationship between \(\mathfrak{so}(3)\) and \(SO(3)\) is given by the exponential map:
\[
\rmR = \exp(\phi^\wedge) = \sum_{n=0}^{\infty} \frac{(\phi^\wedge)^n}{n!}
\]
SO(3) -> so(3): The logarithm map
The inverse of the exponential map is the logarithm map:
\[
\phi = \ln(\rmR)^ \vee = \left(\sum_{n=0}^{\infty} \frac{(-1)^n}{n+1}(\rmR - I)^{n+1}\right)^ \vee
\]
where
\[
|\phi | = \cos^{-1} \left(\frac{trace(R)-1}{2}\right), \frac{\phi }{|\phi |} = \frac{1}{2\sin(|w|)} \left(
\begin{array}{c}
r_{32} - r_{23}\\
r_{13} - r_{31}\\
r_{21} - r_{12}
\end{array}\right)
\]
The above statement says: Any orthongonal transformation \(R\in SO(3)\) can be realized by rotating by an angle \(|\phi|\) around an axis \(\frac{\phi}{|\phi|}\) as defined above.
The Rodrigues' formula can be written as:
\[
R=\exp(\phi^\wedge) = I + \frac{\phi^\wedge}{|\phi|}\sin(|\phi|) + \frac{(\phi^\wedge)^2}{|\phi|^2}(1-\cos(|\phi|))
\]
This formula indicates the exponential map is Rodrigues' formula for rotations in 3D space.
se(3) -> SE(3): The exponential map
For \(\mathrm{SE}(3)\), it also has a corresponding Lie algebra \(\mathfrak{se}(3)\). Similar to \(\mathfrak{so}(3)\), \(\mathfrak{se}(3)\) is located in the \(\mathbb{R}^6\) space:
\[
\mathfrak{se}(3) = \left\{ { \boldsymbol{\xi} = \left[ \begin{array}{l}
\boldsymbol{\rho} \\
\boldsymbol{\phi}
\end{array} \right]
\in { \mathbb{R}^6} ,
\boldsymbol{\rho} \in { \mathbb{R}^3}, \boldsymbol{\phi} \in \mathfrak{so} \left( 3 \right),{ \boldsymbol{\xi} ^ \wedge } = \left[ {\begin{array}{*{20}{c}}
{{ \boldsymbol{\phi} ^ \wedge }}& \boldsymbol{\rho} \\
{{\mathbf{0}^T}}&0
\end{array}} \right] \in { \mathbb{R}^{4 \times 4}}} \right\}.
\]
where \(\boldsymbol{\rho}\) is the translation vector and \(\boldsymbol{\phi}\) is the rotation vector. Here \(^ \wedge\) has a slightly different form than before. The exponential map for \(\mathrm{SE}(3)\) is:
\[
\begin{align}
\exp \left( {{ \boldsymbol{\xi} ^ \wedge }} \right) &= \left[ {\begin{array}{*{20}{c}}
{\sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {{\boldsymbol{\phi} ^ \wedge }} \right)}^n} } }&{\sum\limits_{n = 0}^\infty {\frac{1}{{\left( {n + 1} \right)!}}{{\left( {{\boldsymbol{\phi } ^ \wedge }} \right)}^n} \boldsymbol{\rho} } }\\
{{\mathbf{0}^T}}&1
\end{array}} \right] \\
&\buildrel \Delta \over = \left[ {\begin{array}{*{20}{c}}
\mathbf{R} &{\mathbf{J\rho} } \\
{{\mathbf{0}^T}}&1
\end{array}} \right] = \mathbf{T}.
\end{align}
% \begin{aligned}
% \exp(\boldsymbol{\xi} ^ \wedge) = \left[{\begin{array}{*{20}{c}}
% \exp(\boldsymbol{\phi} ^ \wedge)& \frac{(1-\exp(\boldsymbol{\phi} ^ \wedge)) \boldsymbol{\phi}^\wedge\boldsymbol{\rho} + \boldsymbol{\phi} \boldsymbol{\phi}^\top \boldsymbol{\rho}}{\|\boldsymbol{\phi}\|^2}\\
% 0^\top&1
% \end{array}}\right]
% \end{aligned}
\]
Let \(\boldsymbol{\phi} = \rvtheta\rva\), then
\[
\begin{equation}
\begin{aligned}
\sum\limits_{n = 0}^\infty {\frac{1}{{\left( {n + 1} \right)!}}{{\left( {{\boldsymbol{\phi} ^ \wedge }} \right)}^n}} &= \mathbf{I} + \frac{1}{{2!}}\theta {\mathbf{a}^ \wedge } + \frac{1}{{3 !}}{\theta ^2}{\left( {{\mathbf{a}^ \wedge }} \right)^2} + \frac{1}{{4!}}{\theta ^3}{ \left( {{\mathbf{a}^ \wedge }} \right)^3} + \frac{1}{{5!}}{\theta ^4}{\left( {{\mathbf{a} ^ \wedge }} \right)^4} \cdots \\
&= \frac{1}{\theta }\left( {\frac{1}{{2!}}{\theta ^2} - \frac{1}{{4!}}{\theta ^4} + \cdots } \right)\left( {{\mathbf{a}^ \wedge }} \right) + \frac{1}{\theta }\left( {\frac{1}{{3!}} {\theta ^3} - \frac{1}{5}{\theta ^5} + \cdots } \right){\left( {{\mathbf{a}^ \wedge }} \right)^2} + \mathbf{I}\\
&= \frac{1}{\theta }\left( {1 - \cos \theta } \right)\left( {{\mathbf{a}^ \wedge }} \right) + \frac{{\theta - \sin \theta }}{\theta }\left( {\mathbf{a}{\mathbf{a}^T} - \mathbf{I}} \right) + \mathbf{I}\\
&= \frac{{\sin \theta }}{\theta }\mathbf{I} + \left( {1 - \frac{{\sin \theta }}{\theta }} \right)\mathbf{a }{\mathbf{a}^T} + \frac{{1 - \cos \theta }}{\theta }{\mathbf{a}^ \wedge } \buildrel \Delta \over = \mathbf{J}.
\end{aligned}
\end{equation}
\]
Then \(\rmJ\) can be written as:
\[
\mathbf{J} = \frac{{\sin \theta }}{\theta } \mathbf{I} + \left( {1 - \frac{{\sin \theta }}{\theta }} \right) \mathbf{a} { \mathbf{a}^T} + \frac{{1 - \cos \theta }}{\theta }{ \mathbf{a}^ \wedge }.
\]
Lie Algebra Derivatives and Perturbations
BCH has a linear approximation:
\[
\ln { \left( {\exp \left( { \boldsymbol{\phi} _1^ \wedge } \right)\exp \left( {\boldsymbol{\phi} _2^ \wedge } \right)} \right ) ^ \vee } \approx \left\{
\begin{array}{l}
{\mathbf{J}_l}{\left( {{\boldsymbol{\phi} _2}} \right)^{ - 1}}{ \boldsymbol{\phi} _1} + {\boldsymbol{\phi} _2 } \quad \text{when} \ \boldsymbol{\phi}_1 \ \text{is a small amount},\\
{\mathbf{J}_r}{\left( {{\boldsymbol{\phi} _1}} \right)^{ - 1}}{\boldsymbol{\phi} _2} + {\boldsymbol{\phi} _1 } \quad \text{when} \ \boldsymbol{\phi}_2 \ \text{is a small amount}.
\end{array} \right.
\]
The first formula tells us that that left multiplying a tiny rotation matrix \(\mathbf{R}_1\) on a rotation matrix \(\mathbf{R}_2\), in \(\mathfrak{so}(3)\) it can be approximated by adding a \(\mathbf{J}_l \left( {\boldsymbol{\phi} _2} \right)^{ - 1} { \boldsymbol{\phi} _1}\) to the original Lie algebra \(\boldsymbol{\phi}_2\). Similarly, the second approximation describes the case where \(\mathbf{R}_1\) is right multiplied by a small rotation. The jacobian in left model \(\mathbf{J}_l\) is:
\[
\begin{equation}
{ \mathbf{J}_l} = \mathbf{J} = \frac{{\sin \theta }}{\theta } \mathbf{I} + \left( {1 - \frac{{\sin \theta } }{\theta }} \right) \mathbf{a} { \mathbf{a}^T} + \frac{{1 - \cos \theta }}{\theta }{ \mathbf{a} ^ \wedge}.
\end{equation}
\]
The jacobian in right model \(\mathbf{J}_r\) is:
\[
\mathbf{J}_r(\boldsymbol{\phi}) =\mathbf{J}_l(-\boldsymbol{\phi}) .
\]
Let us say if we have a rotation \(\mathbf{R}\), the corresponding Lie algebra is \(\boldsymbol{\phi}\). We give it a small perturbation to the left, denoted as \(\Delta \mathbf{R}\), and so that the corresponding Lie algebra is \(\Delta \boldsymbol{\phi}\). Then, on Lie group, the result is $ \Delta \mathbf{R} \cdot \mathbf{R}$, and on the Lie algebra, according to the BCH approximation, it is \(\mathbf{J}_l^{-1 } (\boldsymbol{\phi}) \Delta \boldsymbol{\phi} + \boldsymbol{\phi}\). Putting them together, we can simply write:
\[
\exp \left( {\Delta { \boldsymbol{\phi} ^ \wedge }} \right)\exp \left( {{ \boldsymbol{\phi} ^ \wedge }} \right) = \exp \left( {{{\left( { \boldsymbol{\phi} + \mathbf{J}_l^{ - 1}\left( \boldsymbol{\phi} \right)\Delta \boldsymbol{\phi} } \right)} ^ \wedge }} \right).
\]
For \(\mathfrak{se}(3)\), the BCH formula is:
\[
\exp \left( {\Delta {\boldsymbol{\xi} ^ \wedge }} \right)\exp \left( {{ \boldsymbol{\xi} ^ \wedge }} \right) \approx \exp \left ( {{{\left( {{ \boldsymbol{\mathcal{J}}_l^{-1} }\Delta \boldsymbol{\xi} + \boldsymbol{\xi} } \right)}^ \wedge }} \right),
\]
Here the \(\boldsymbol{\mathcal{J}}_l\) and \(\boldsymbol{\mathcal{J}}_r\) are more complicated \(6 \times 6\) matrices
Derivative of Lie Algebra
Let us say we have a observation \(\rvz\) of the world point \(\rvp\) with camera pose \(\rmT\). We want to minimize the error between the observation and the projection of the world point. The error is defined as:
\[
\mathop {\min }\limits_{\mathbf{T}} J(\mathbf{T} ) = \sum_{i=1}^{N} \left\| {\mathbf{z}_i - \mathbf{Tp}_i} \right\|^2_2
\]
Since \(\mathrm{SO}(3)\) and \(\mathrm{SE}(3)\) are Lie groups that do not have addition and subtraction. Thus, we need instead use Lie algebra.
Derivative of SO(3)
Consider the case:
\[
\frac{{\partial \left( {\mathbf{Rp}} \right)}}{{\partial \mathbf{R}}}
\]
We can use Lie algebra:
\[
\frac{{\partial \left( {\exp \left( \boldsymbol{\phi} ^ \wedge \right) \mathbf{p}} \right)}}{{\partial \boldsymbol{\phi} }}
\]
By solving this, we can get:
\[
\begin{align*}
\frac{{\partial \left( {\exp \left( {{ \boldsymbol{\phi} ^ \wedge }} \right) \mathbf{p}} \right)}}{{\partial \boldsymbol{\phi} }} &= \mathop {\lim }\limits_{\delta \boldsymbol{\phi} \to \mathbf{0}} \frac{{\exp \left( {{{\left( {\boldsymbol{\phi} + \delta \boldsymbol{\phi} } \right)}^ \wedge }} \right) \mathbf{p} - \exp \left( {{\boldsymbol{\phi} ^ \wedge }} \right)\mathbf{p}}}{{\delta \boldsymbol{\phi} }} \\
& = \mathop {\lim }\limits_{\delta \boldsymbol{\phi} \to \mathbf{0}} \frac{{\exp \left( {{{\left( {{\mathbf{J}_l}\delta \boldsymbol{\phi} } \right)}^ \wedge }} \right)\exp \left( {{\boldsymbol{\phi} ^ \wedge }} \right) \mathbf{p} - \exp \left( {{\boldsymbol{\phi} ^ \wedge }} \right) \mathbf{p}}}{{\delta \boldsymbol{\phi} }}\quad \text{BCH approximation, inverse?}\\
&\approx \mathop {\lim }\limits_{\delta \boldsymbol{\phi} \to \mathbf{0}} \frac{{\left( { \mathbf{I} + {{\left( {{ \mathbf{J}_l}\delta \boldsymbol{\phi} } \right)}^ \wedge }} \right)\exp \left( {{\boldsymbol{\phi} ^ \wedge }} \right) \mathbf{p} - \exp \left( {{\boldsymbol{\phi} ^ \wedge }} \right)\mathbf{p}}}{{\delta \boldsymbol{\phi} }}\quad \text{Taylor expansion}\\
&= \mathop {\lim }\limits_{\delta \boldsymbol{\phi} \to \mathbf{0}} \frac{{{{\left( {{\mathbf{J}_l}\delta \boldsymbol{\phi} } \right)}^ \wedge }\exp \left( {{\boldsymbol{\phi} ^ \wedge }} \right)\mathbf{p}}}{{\delta \boldsymbol{\phi} }}\\
&= \mathop {\lim }\limits_{\delta \boldsymbol{\phi} \to \mathbf{0}} \frac{{ - {{\left( {\exp \left( {{\boldsymbol{\phi} ^ \wedge }} \right)\mathbf{p}} \right)}^ \wedge }{\mathbf{J}_l}\delta \boldsymbol{\phi} }}{{\delta \boldsymbol{\phi}}} \quad\text{$^\wedge$ can be seen as cross product}\\
&= - {\left( {\mathbf{Rp}} \right)^ \wedge }{\mathbf{J}_l}.
\end{align*}
\]
Perturbation model (left perturbation) of SO(3)
We can perturb \(\mathbf{R}\) by \(\Delta \mathbf{R}\) (left/right). Let's take the left perturbation as an example. Let the left perturbation \(\Delta \mathbf{R}\) correspond to the Lie algebra as \(\boldsymbol{\varphi}\). Then, for \(\boldsymbol{\varphi}\), that is:
\[
\begin{align*}
\frac{{\partial \left( {\mathbf{Rp}} \right)}}{{\partial \boldsymbol{\varphi} }} &= \mathop {\lim }\limits_{\boldsymbol{\varphi} \to \mathbf{0}} \frac{{\exp \left( {{\boldsymbol{\varphi} ^ \wedge }} \right)\exp \left( {{\boldsymbol{\phi} ^ \wedge }} \right)\mathbf{p} - \exp \left( {{\boldsymbol{\phi} ^ \wedge }} \right)\mathbf{p}}}{ \boldsymbol{\varphi} }\\
&= \mathop {\lim }\limits_{\boldsymbol{\varphi } \to \mathbf{0}} \frac{{\left( {\mathbf{I} + {\boldsymbol{\varphi }^ \wedge }} \right)\exp \left( {{\boldsymbol{\phi} ^ \wedge }} \right)\mathbf{p} - \exp \left( {{\boldsymbol{\phi} ^ \wedge }} \right)\mathbf{p}}}{\boldsymbol{\varphi} }\\
&= \mathop {\lim }\limits_{\boldsymbol{\varphi} \to \mathbf{0}} \frac{{{\boldsymbol{\varphi} ^ \wedge }\mathbf{Rp}}}{\boldsymbol{\varphi} } = \mathop {\lim }\limits_{\boldsymbol{\varphi} \to \mathbf{0}} \frac{{ - {{\left( \mathbf{Rp} \right)}^ \wedge }\boldsymbol{\varphi} }}{\boldsymbol{\varphi} } = - {\left( \mathbf{Rp} \right)^ \wedge }
\end{align*}
\]
Perturbation model (left perturbation) of SE(3)
Let us say a point \(\mathbf{p}\) is transformed by \(\mathbf{T}\) (corresponding to Lie algebra \(\boldsymbol{\xi}\)), and the result is \(\mathbf{Tp}\). Now, give \(\mathbf{T}\) a left perturbation \(\Delta \mathbf{T} = \exp \left( \delta \boldsymbol{\xi}^\wedge \right)\), whose Lie algebra is \(\delta \boldsymbol{\xi} = [\delta \boldsymbol{\rho}, \delta \boldsymbol{\phi}]^T\), then:
\[
\begin{align*}
\frac{{\partial \left( \mathbf{Tp} \right)}}{{\partial \delta \boldsymbol{\xi}}} &= \mathop {\lim }\limits_{\delta \boldsymbol{\xi} \to \mathbf{0}} \frac{{\exp \left( {\delta {\boldsymbol{\xi} ^ \wedge }} \right)\exp \left( {{\boldsymbol{\xi} ^ \wedge }} \right) \mathbf{p} - \exp \left( {{\boldsymbol{\xi} ^ \wedge }} \right) \mathbf{p} }}{{\delta \boldsymbol{\xi} }}\\
&= \mathop {\lim }\limits_{\delta \boldsymbol{\xi} \to \mathbf{0}} \frac{{\left( { \mathbf{I} + \delta {\boldsymbol{\xi} ^ \wedge }} \right)\exp \left( {{\boldsymbol{\xi} ^ \wedge }} \right) \mathbf{p} - \exp \left( {{\boldsymbol{\xi} ^ \wedge }} \right) \mathbf{p} }}{{\delta {\boldsymbol{\xi} }}}\\
&= \mathop {\lim }\limits_{\delta \boldsymbol{\xi} \to \mathbf{0}} \frac{{\delta {\boldsymbol{\xi} ^ \wedge }\exp \left( {{\boldsymbol{\xi} ^ \wedge }} \right) \mathbf{p}}}{{\delta {\boldsymbol{\xi} }}}\\
&= \mathop {\lim }\limits_{\delta \boldsymbol{\xi} \to \mathbf{0}}
\frac{{\left[ {\begin{array}{*{20}{c}}
{\delta { \boldsymbol{\phi} ^ \wedge }}&{\delta {\boldsymbol{\rho} }}\\
{{\mathbf{0}^T}}&0
\end{array}} \right]\left[ {\begin{array}{*{20}{c}}
{\mathbf{Rp} + \mathbf{t}}\\
1
\end{array}} \right]}}{{\delta {\boldsymbol{\xi} }}}\\
&= \mathop {\lim }\limits_{\delta \boldsymbol{\xi} \to \mathbf{0}} \frac{{\left[ {\begin{array}{*{20}{c}}
{\delta {\boldsymbol{\phi} ^ \wedge }\left( {\mathbf{Rp} + \mathbf{t}} \right) + \delta {\boldsymbol{\rho} }}\\
\mathbf{0}^T
\end{array}} \right]}}{[\delta\boldsymbol{\rho},\delta \boldsymbol{\phi}]^T} = \left[ {\begin{array}{*{20}{c}}
\mathbf{I} & { - {{\left( {\mathbf{Rp} + \mathbf{t}} \right)}^ \wedge }} \\
{{\mathbf{0}}}^T & \mathbf{0}^T
\end{array}} \right] \buildrel \Delta \over = {\left( \mathbf{Tp} \right)^ \odot }.
\end{align*}
\]
References
1 Computer vision II, Lecture 2, Prof. Daniel Cremers, Technical University of Munich