课程的 YouTube 地址为：https://www.youtube.com/playlist?list=PLTBdjV_4f-EJn6udZ34tht9EVIW7lbeo4 。视频评论区可以找到课程所使用课件与练习题的下载地址。

课程第2章从李群与李代数的角度介绍三维空间的刚体运动。李群即常见的旋转矩阵、变换矩阵，李代数与李群对应，李代数 $se(3)$ 是所有三维反对称阵的集合。

将李代数映射到李群，使得旋转与变换可微，并且消除了旋转矩阵的约束条件（$ R^TR = I, det(R) = 1 $）对优化的约束，能够在无约束条件下进行位姿的优化。

1. 三维空间与刚体运动

三维空间是三维欧几里德空间（Euclidean Space）的简称，一般使用笛卡尔坐标系（Cartesian Coordinate System）描述欧式空间中的点，笛卡尔坐标描述是 $ (x, y, z) \in \mathbb{E}^3 $ 的形式，这种形式可以用 $\mathbb{R}^3$ 定义。

于是使用三维向量 $ \mathbb{R}^3 $ 描述一个三维点坐标 $ \mathbb{E}^3 $。本笔记使用 $\mathbb{R}^3$ 替代 $ \mathbb{E}^3 $，严格意义上将 $\mathbb{E}^3$ 是笛卡尔坐标，$ \mathbb{R}^3 $ 是三维向量。

1.1 叉乘与反对称阵

叉乘（Cross Product）是将两个三维向量映射到一个三维向量：

\[ \times : \mathbb{R}^3 \times \mathbb{R}^3 \rightarrow \mathbb{R}^3 : u \times v = \begin{bmatrix} u_2v_3 - u_3v_2 \\ u_3v_1 - u_1v_3 \\ u_1v_2 - u_2v_1 \end{bmatrix} \in \mathbb{R}^3 \]

叉乘可以可以转化为一个矩阵与向量点乘的形式，方便计算：

\[ u \times v = \hat{u} \cdot v, \hat{u} = \begin{bmatrix} 0 \quad -u_3 \quad u_2 \\ u_3 \quad 0 \quad -u_1 \\ -u_2 \quad u_1 \quad 0 \end{bmatrix} \]

1.2 刚体运动

刚体运动是三维坐标到三维坐标的映射：

\[ g_t : \mathbb{R}^3 \rightarrow \mathbb{R}^3; X \mapsto g_t(X), t \in [0, T] \]

$ t $ 是时刻。

刚体运动保持范数（norm）与叉乘不变：

\[ | g_t(v) | = | v |, \forall v \in \mathbb{R}^3 \]

\[ g_t(u) \times g_t(v) = g_t(u \times v), \forall u, v \in \mathbb{R}^3 \]

由以上两个性质与极化恒等式（Polarization Identity）可知刚体运动也保持内积不变。三重积（Triple Product）也是保持变不变，三重积的几何意义是三个向量表示的平行六面体的体积，所以刚体运动保持体积不变。

\[ <g_t(u), g_t(v) \times g_t(w)> = <u, v \times w> \]

刚体运动可以表示为：

\[ g_t(x) = Rx + T \]

2. 李群与李代数

2.1 旋转矩阵的导数

在任意时刻的旋转矩阵都是正交的，$ R(t)R^T(t) = I, \forall t $，对这个式子左右求导：

\[ {d \over dt}(RR^T) = \dot RR^T + R {\dot R}^T = 0 \Rightarrow \dot RR^T = -(\dot RR^T)^T \]

所以 $\dot RR^T$ 是一个反对称阵，所以在任意时刻存在一个向量 $w(t) \in \mathbb{R}^3$ 与 $ \dot RR^T $ 对应：

\[ \dot R(t)R^T(t) = \hat w(t) \Leftrightarrow \dot R(t) = \hat w R(t) \]

上式表明了任意时刻 $ t $ 的旋转矩阵 $R(t)$ 的导数 $\dot R(t)$ 的计算方法——用一个反对称阵左乘旋转矩阵 $R(t)$。

0时刻的旋转矩阵 $ R(0) = I $，在0时刻附近展开：

\[ R(dt) = R(0) + dR = I + \hat w(0) dt \]

2.2 李群 $SO(3)$ 与李代数 $so(3)$

李群（Lie Group）指连续可微的群，三维空间的旋转是连续的，所以特殊正交集 $SO(3)$ 是三维旋转李群对应的矩阵表示。连续是关键。

Def.: A Lie group (or infinitesimal group) is a smooth manifold that is also a group, such that the group operations multiplication and inversion are smooth maps.

$ SO(3) $ 中的微分可以使用反对称阵逼近，反对称阵所在的群叫做李代数（Lie Algebra）：

\[ so(3) \equiv \left\{ \hat w \right. \left| w \in \mathbb{R}^3 \right\} \]

李代数是李群在 $I$ 处的切空间（Tangent Space）。$ I $ 对应的是0时刻，以0时刻为基准，认为0时刻不存在旋转。

2.2.1 指数映射

前面推导得到了 $ \dot R(t) = \hat w R(t) $ 的结论，解微分方程

\[ \begin{cases} \dot R(t) = \hat w R(t) \\ R(0) = I\end{cases} \]

得到

\[ R(t) = e^{\hat w t} = \Sigma_{n = 0}^{\infty}{ {(\hat w t)}^n \over n !} = I + \hat w t + {{(\hat w t)}^2\over 2!} + \dots \]

当 $ |w| = 1 $ 时 $R(t)$ 是绕着轴 $ w \in \mathbb{R}^3 $ 的旋转，如果将时间 $t$ 写入到 $\hat w$ 中（$ \hat w = \hat w t $），这样就得到了从李代数到李群的映射：

\[ exp: so(3) \rightarrow SO(3); \hat w \mapsto e^{\hat w} \]

2.2.2 对数映射

对数映射是将指数映射的逆映射，是从李群 $ SO(3) $ 到李代数 $ so(3) $ 的映射，可以表示为

\[ log: SO(3) \rightarrow so(3); R \mapsto \hat w \]

当 $ R \ne I $ 时，
\[ \|w\| = {\cos}^{-1}\left. \right({trace(R) - 1\over 2}\left. \right), {w \over \| w \|} = {1 \over \sin(\|w\|)} \begin{bmatrix} r_{32} - r_{23} \\ r_{13} - r_{31} \\ r_{21} - r_{12} \end{bmatrix} \]
当 $ R = I $ 时，\[ w = 0 \]

2.2.3 罗德里格公式

指数映射部分将 $R(t)$ 用一无穷级数表示，如何求取这个无穷级数呢？这就需要用到罗格里格公式（Rodrigues' Rotation Formula），对于 $ \hat w \in so(3) $：

\[ e^{\hat w} = I + {\hat w \over \| w \|}\sin(\| w \|) + {\hat w^2 \over \| w \|^2}(1- \cos(\| w \|)) \]

2.3 李群 $SE(3)$ 与李代数 $se(3)$

$ SE(3) $ 表示刚体运动，包括旋转和平移，在齐次坐标下定义为：

\[ SE(3) \equiv \left\{ g = \begin{bmatrix} R \quad T \\ 0 \quad 1 \end{bmatrix}\left. \right| R \in SO(3), T \in \mathbb{R}^3 \right\} \subset \mathbb{R}^{4 \times 4}\]

旋转矩阵求导能够直接得出旋转矩阵的导数 $ \dot R(t) = \hat w R(t) $，但 $ SE(3) $ 不具有这种性质，为了保持表达形式的一致，仿照 $SO(3)$ 与 $so(3)$ 定义 $se(3)$。

$ g $ 是刚体变换映射：

\[ g: \mathbb{R} \rightarrow SE(3); g(t) = \begin{bmatrix} R(t) & T(t) \\ 0 & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \]

$ g $ 是一个 $4 \times 4$ 的矩阵，但是并不是一个正交阵，不具备 $ g g^T = I $ 的性质，但是作为一个可逆矩阵 $ gg^{-1} = I \Rightarrow \dot g g^{-1} = (\dot g g)^{-1}$，然后就如下考虑：

\[ \dot g (t) g^{-1}(t) = \begin{bmatrix} \dot R R^T & \dot T - \dot R R^T T \\ 0 & 0\end{bmatrix} \in \mathbb{R}^{4\times4} \]

前面 $ SO(3) $ 部分可知 $ \dot R R^T = \hat w, \hat w\in so(3) $，定义一个新的向量 $ v(t) \equiv \dot T(t) - \hat w(t)T(t) $，于是有：

\[ \dot g (t) g^{-1}(t) = \begin{bmatrix} \hat w(t) & v(t) \\ 0 & 0\end{bmatrix} \equiv \hat \xi(t) \in \mathbb{R}^{4\times4} \]

进一步求 $\dot g$：

\[ \dot g = \dot g g^{-1}g = \hat \xi g \]

公式中的 $\hat\xi$ 称作 twist （twist 有旋转运动加平移运动的意思，可查 Screw Theory）。

$se(3)$ 的定义：

\[ se(3) \equiv \left\{ \hat \xi = \begin{bmatrix} \hat w & v \\ 0 & 0 \end{bmatrix} \left. \right| \hat w \in so(3), v \in \mathbb{R}^3 \right\} \subset \mathbb{R}^{4 \times 4} \]

$ \hat \xi $ 是 twist，$ \xi \in \mathbb{R}^6 $ 称作 twist coordinates，两者的完整定义如下：

\[ \hat \xi \equiv { \begin{bmatrix} v \\ w \end{bmatrix} }^{\hat{}} \equiv \begin{bmatrix} \hat w & v \\ 0 & 0 \end{bmatrix} \in \mathbb{R}^{4\times4} \]

\[ \xi \equiv { \begin{bmatrix} v \\ w \end{bmatrix} } \equiv {\begin{bmatrix} \hat w & v \\ 0 & 0 \end{bmatrix}}^{\check{}} \in \mathbb{R}^{6} \]

$ v $ 是在 $R$ 旋转后的坐标系的线速度，$w$ 是角速度。