Lie Group and Lie Algebra: SO(3), SE(3)

Overview ^[1]

Matrix Exponential & Logarithm

已知 \(A \in \mathbb{R}^{M \times M}\),

\[ \exp (\mathbf{A}) =\sum_{n=0}^{\infty} \frac{1}{n !} \mathbf{A}^{n} =\mathbf{1}+\mathbf{A}+\frac{1}{2 !} \mathbf{A}^{2}+\frac{1}{3 !} \mathbf{A}^{3}+\cdots \]

\[ \ln (\mathbf{A})=\sum_{n=1}^{\infty} \frac{(-1)^{n-1}}{n}(\mathbf{A}-\mathbf{1})^{n} \]

Lie Bracket

TODO

\(SU(2)\) and the Quaternions ^[2]

\[ R = C(q) \longrightarrow R^T = C(q^{-1}) \]

欧拉公式

\[ e^{ix} = \cos x + i \sin x \]

2D旋转

单位复数 可用来表示2D旋转。

\[ z = a + b\vec{i} = r (\cos \theta + \sin \theta \vec{i} ) = e^{\theta \vec{i}}, \quad r = ||z||=1 \]

3D旋转

单位四元数 才可表示3D旋转，四元数是复数的扩充，在表示旋转前需要进行 归一化。

\[ \mathbf{q} = \exp(\frac{\boldsymbol \phi}{2}) = \exp \left( \frac{\mathbf{u} \theta}{2} \right) = \cos \frac{\theta}{2} + \mathbf{u} \sin \frac{\theta}{2} = \begin{bmatrix} \cos \frac{\theta}{2} \\ \mathbf{u} \sin \frac{\theta}{2} \end{bmatrix} \quad s.t. \quad ||\mathbf{q}||_2 = 1 \]

当 \(\theta\) 很小时，一阶泰勒展开，可以近似表达为

\[ \mathbf{q} = \exp({\frac{\mathbf{u}\theta}{2}}) \approx 1 + \frac{\mathbf{u}\theta}{2} = \begin{bmatrix} 1 \\ \frac{\mathbf{u}\theta}{2} \end{bmatrix} = \begin{bmatrix} 1 \\ \frac{\boldsymbol{\phi}}{2} \end{bmatrix} \]

四元数可以在 保证效率 的同时，减小矩阵1/4的内存占有量，同时又能 避免欧拉角的万向锁问题。

Hamilton & JPL 四元数

\(SO(3)\)

Lie Group \(SO(3)\)

\[ SO(3) = \Bigg\{ \mathbf{R} \in \mathbb{R}^{3 \times 3} \Bigg| \mathbf{RR}^T = \mathbf{I}, det(\mathbf{R}) = 1 \Bigg\} \]

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

\[ \mathfrak{so}(3) = \Bigg\{ \boldsymbol{\Phi} = \boldsymbol{\phi}^{\wedge} \in \mathbb{R}^{3 \times 3} \Bigg| \boldsymbol{\phi} \in \mathbb{R}^3 \Bigg\} \]

where

\[ \boldsymbol{\phi}^{\wedge} = \begin{bmatrix} \phi_1 \\ \phi_2 \\ \phi_3 \end{bmatrix}^{\wedge} = \begin{bmatrix} 0 & -\phi_3 & \phi_2 \\ \phi_3 & 0 & -\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix} \in \mathbb{R}^{3 \times 3} \]

Infinitesimal Rotations

the base of \(\mathfrak{so}(3)\) are three skew symmetric matrices, each corresponding to infinitesimal rotations along each axis

\[ \mathbf{G}_{1}^{\mathfrak{s o}(3)}=\mathbf{e}_{1}^{\wedge}=\left(\begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{array}\right) \quad \mathbf{e}_{1}=\left[\begin{array}{l} 1 \\ 0 \\ 0 \end{array}\right] \]

\[ \mathbf{G}_{2}^{\mathfrak{s o}(\mathbf{3})}=\mathbf{e}_{2}^{\wedge}=\left(\begin{array}{ccc} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0 \end{array}\right) \quad \mathbf{e}_{2}=\left[\begin{array}{l} 0 \\ 1 \\ 0 \end{array}\right] \]

\[ \mathbf{G}_{3}^{\mathfrak{s o}(3)}=\mathbf{e}_{3}^{\wedge}=\left(\begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right) \quad \mathbf{e}_{3}=\left[\begin{array}{l} 0 \\ 0 \\ 1 \end{array}\right] \]

then the \(\mathfrak{so}\)(3)

\[ \mathfrak{s o}(3)=\left\{\mathbf{G}_{\mathbf{i}}^{\mathfrak{s o}(\mathbf{3})}\right\}_{i=1,2,3} \]

映射

指数映射

\[ \begin{aligned} \mathbf{R} &= \exp(\boldsymbol{\phi}^{\wedge}) \\ &= \mathbf{I} + \boldsymbol{\phi}^{\wedge} \mathbf{J}_l \quad \text{???} \end{aligned} \]

当 \(\|\phi\|\) 比较小时，一阶泰勒近似

\[ \mathbf{R} \approx \mathbf{I} + \boldsymbol{\phi}^{\wedge} \]

对数映射

\[ \boldsymbol{\phi} = \log(\mathbf{R})^{\vee} \]

\(SE(3)\)

Lie Group \(SE(3)\)

\[ SE(3) = \Bigg\{ \mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \Bigg| \mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^{3} \Bigg\} \]

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

\[ \mathfrak{se}(3) = \Bigg\{ \boldsymbol{\Xi} = \boldsymbol{\xi}^{\wedge} \in \mathbb{R}^{4 \times 4} \Bigg| \boldsymbol{\xi} \in \mathbb{R}^6 \Bigg\} \]

where

\[ \boldsymbol{\xi}^{\wedge} = \begin{bmatrix} \boldsymbol{\rho} \\ \boldsymbol{\phi} \end{bmatrix}^{\wedge} = \begin{bmatrix} \boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^T, & 0 \end{bmatrix} \in \mathbb{R}^{4 \times 4}, \quad \boldsymbol{\rho},\boldsymbol{\phi} \in \mathbb{R}^3 \]

The infinitesimal generators of SE(3)

the base of \(\mathfrak{se}(3)\) are these six 4×4 matrices, each corresponding to either infinitesimal rotations or infinitesimal translations along each axis

\[ \mathbf{G}_{\{\mathbf{1 , 2 , 3}\}}^{\mathfrak{s e}(3)} =\left(\begin{array}{c|c} & 0 \\ \mathbf{G}_{\{\mathbf{1}, \mathbf{2}, \mathbf{3}\}}^{\mathfrak{s o}(3)} & 0 \\ & 0 \\ \hline 0 & 0 \end{array}\right) \]

\[ \mathbf{G}_{4}^{\mathfrak{s c}(3)} =\left(\begin{array}{c|c} & 1 \\ \mathbf{0}_{3 \times 3} & 0 \\ & 0 \\ \hline 0 & 0 \end{array}\right) \quad \quad \mathbf{G}_{5}^{\mathfrak{s c}(3)}=\left(\begin{array}{c|c} & 0 \\ \mathbf{0}_{3 \times 3} & 1 \\ & 0 \\ \hline 0 & 0 \end{array}\right) \quad \quad \mathbf{G}_{6}^{\mathfrak{s c}(3)}=\left(\begin{array}{c|c} & 0 \\ \mathbf{0}_{3 \times 3} & 0 \\ & 1 \\ \hline 0 & 0 \end{array}\right) \]

then the \(\mathfrak{se}(3)\)

\[ \mathfrak{s e}(3)=\left\{\mathbf{G}_{\mathbf{i}}^{\mathfrak{s e}(3)}\right\}_{i=1 \ldots 6} \]

so

\[ \xi^\wedge = \sum_{i=1}^6 \xi(i) \cdot G_i \]

Sophus::Matrix4d sk_b1;
sk_b1.setZero();
for (int i = 0; i < vec_b.size(); i++) {
    sk_b1 += vec_b[i] * Sophus::SE3d::generator(i);
}

Jacobian

to convert the translation component of pose in \(\mathfrak{se}(3)\) into the translation component of pose in \(SE(3)\) through

\[ \mathbf{t}=\mathbf{J}_l \boldsymbol{\rho} \in \mathbb{R}^{3}, \quad \mathbf{J}_l=\sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\phi^{\wedge}\right)^{n} \]

映射

指数映射

\[ \mathbf{T} = \exp(\boldsymbol{\xi}^{\wedge}) \]

对数映射

\[ \boldsymbol{\xi} = \log(\mathbf{T})^{\vee} \]

Adjoints

Adjoint action of SE(3)

• https://gtsam.org/2021/02/23/uncertainties-part3.html

上图用伴随表示：

\[ \exp(\xi_w^{\wedge}) = T_{wb} \cdot \exp(\xi_b^{\wedge}) \cdot T_{wb}^{-1} = \exp((\mathtt{Adj}_{T_{wb}} \cdot \xi_b)^{\wedge}) \]

with the key property of exponential map

\[ \exp(T \xi^\wedge T^{-1}) = T \cdot \exp(\xi^\wedge) \cdot T^{-1} \]

we can get

\[ T \xi^\wedge T^{-1} = (\mathtt{Adj}_{T} \cdot \xi)^{\wedge} \]

so, for \(SO(3)\)

\[ R \phi^{\wedge} R^T = (R \phi)^{\wedge} \]

同一刚体中不同坐标系姿态变换的相互表示

以带有IMU的相机模组为例，已知 IMU（坐标系）本身的姿态变换 \(\mathbf{T}^{B}\) 和 同一模组中Camera到IMU(Body)的坐标系变换 \(\mathbf{T}_{BC}\)，则 该Camera（坐标系）本身的姿态变换为：

\[ {}_C\mathbf{T} = \mathbf{T}_{BC} \cdot \mathbf{T}^{B} \cdot \mathbf{T}_{BC}^{-1} \]

因为上面的变换都是 坐标系的变换，所以矩阵相乘 从左到右，即 矩阵右乘

Exponential Map on \(SE(3)\)

Eigen::Matrix3d R_b = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d(0, 1, 0)).toRotationMatrix();
Sophus::SO3d SO3_b(R_b);
Sophus::SE3d::Tangent vec_b;
vec_b.head(3) << 10.1793, -6.3204, 28.09113;
vec_b.tail(3) = SO3_b.log();

Sophus::Vector3d t_wb(1.2, 3.4, 5.6);
Sophus::SE3d SE3_wb(R, t_wb);

Sophus::SE3d SE3_w = SE3_wb * Sophus::SE3d::exp(vec_b) * SE3_wb.inverse();
Sophus::SE3d::Tangent vec_w0 = SE3_w.log();

Sophus::SE3d::Tangent vec_w1 = Sophus::SE3d::vee(SE3_wb.matrix() * Sophus::SE3d::hat(vec_b) * SE3_wb.inverse().matrix());

Sophus::SE3d::Tangent vec_w2 = Sophus::SE3d::exp(vec_w1).log();

Sophus::SE3d::Tangent vec_b_adj = SE3_wb.Adj() * vec_b;

std::cout << "vec_w0 :" << vec_w0.transpose() << std::endl;
std::cout << "vec_w1 :" << vec_w1.transpose() << std::endl;
std::cout << "vec_w2 :" << vec_w2.transpose() << std::endl;
std::cout << "vec_b_adj :" << vec_b_adj.transpose() << std::endl;

output:

vec_w0 :     6.3204     5.78107     30.7615   -0.785398 1.13928e-16           0
vec_w1 :     6.3204     5.78107     30.7615   -0.785398 1.74393e-16           0
vec_w2 :     6.3204     5.78107     30.7615   -0.785398 1.74393e-16           0
vec_b_adj :  6.3204     5.78107     30.7615   -0.785398 1.74393e-16           0

Baker-Campbell-Hausdorff (BCH)

Properties

\[ \exp((\phi + \delta \phi)^{\wedge}) \approx \exp((J_l \delta \phi)^{\wedge}) \cdot \exp({\phi}^{\wedge}) = \exp((J_l \delta \phi)^{\wedge}) \cdot R \]

\[ \exp((\phi + \delta \phi)^{\wedge}) \approx \exp({\phi}^{\wedge}) \cdot \exp((J_r \delta \phi)^{\wedge}) = R \cdot \exp((J_r \delta \phi)^{\wedge}) \]

where

\[ J_l = J_l (\phi), \quad J_r = J_r (\phi) \]

and

\[ \boldsymbol{J}_{l}=\boldsymbol{J}=\frac{\sin \phi}{\phi} \boldsymbol{I}+\left(1-\frac{\sin \phi}{\phi}\right) \boldsymbol{a} \boldsymbol{a}^{T}+\frac{1-\cos \phi}{\phi} \boldsymbol{a}^{\wedge} \]

\[ \boldsymbol{J}_{r}=\boldsymbol{J}=\frac{\sin \phi}{\phi} \boldsymbol{I}+\left(1-\frac{\sin \phi}{\phi}\right) \boldsymbol{a} \boldsymbol{a}^{T}-\frac{1-\cos \phi}{\phi} \boldsymbol{a}^{\wedge} \]

so

\[ J_l (-\phi) = J_r (\phi) \]

当 \(\phi\) 很小时

\[ J_l \approx I, \quad J_r \approx I \]

Rotations

The BCH formula

\[ \begin{aligned} \ln \left(\mathbf{C}_{1} \mathbf{C}_{2}\right)^{\vee}=\ln (\exp (&\left.\left.\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee} \\ & \approx\left\{\begin{array}{ll} \mathbf{J}_{\ell}\left(\phi_{2}\right)^{-1} \phi_{1}+\phi_{2} & \text { if } \phi_{1} \text { small } \\ \phi_{1}+\mathbf{J}_{r}\left(\phi_{1}\right)^{-1} \phi_{2} & \text { if } \phi_{2} \text { small } \end{array},\right. \end{aligned} \]

In Lie group theory, \(J_r\) and \(J_l\) are referred to as the right and left Jacobians of \(SO(3)\), respectively.

• 四元数矩阵与 so(3) 左右雅可比

Libs

• Sophus
• manif: A small header-only library for Lie theory

Reference

1. 第四讲：李群和李代数 ↩
2. SU(2) and the quaternions ↩