笔记：李群与李代数求导

最近一段时间在推\(Jacobian\)，会用到一些关于李代数求导的知识。参考高博《视觉slam十四讲》一书，在此总结一些常用的关于李群与李代数相关的知识点。

前言

在SLAM中位姿是未知的，而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的\(R\),\(t\)，使得误差最小化。 旋转矩阵自身是带有约束的（正交且行列式为 1）。它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群——李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。群（Group）是一种集合加上一种运算的代数结构，李群是指具有连续（光滑）性质的群，李群在相机姿态估计时具有重要意义，接下来主要讨论特殊正交群\(SO(n)\)与特殊欧式群\(SE(n)\)。

特殊正交群与特殊欧式群

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

\[ SE(3) = \left\{ \mathbf{T} = \left[ {\begin{array}{*{20}{c}} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{array}} \right] \in \mathbb{R}^{4 \times 4} | \mathbf{R} \in SO(3), \mathbf{t} \in \mathbb{R}^3\right\} \] 在李群中，我们使用矩阵来表达一个旋转和平移，这存在冗余的自由度。三维空间的旋转只有三自由度，旋转+平移有六自由度。因此，我们希望寻找一个没有冗余自由度（但是相应的存在奇异性）的表示，也就是李代数\(\mathfrak{so}(3)\)和\(\mathfrak{se}(3)\)。且无论是旋转还是变换矩阵，它们都是对加法不封闭的，但是对乘法是封闭的。

李代数的引出

关于李代数理论的介绍，包含李代数/切空间/李群的介绍，非常形象。

对于任意旋转矩阵\(R\)，它必定满足：

\[\mathbf{R} \mathbf{R}^T＝\mathbf{I}.\]

考虑它随时间发生变化，即从\(\mathbf{R}\)变为\(\mathbf{R(t)}\)，它仍然满足如下如下等式：

\[\mathbf{R}(t) \mathbf{R}(t) ^T = \mathbf{I}\]

对两侧同时对时间求导数得：

\[\mathbf{\dot{R}} (t) \mathbf{R} {(t)^T} + \mathbf{R} (t) \mathbf{\dot{R}} {(t)^T} = 0 \] 则有： \[\mathbf{\dot{R}} (t) \mathbf{R} {(t)^T} = - \left( \mathbf{\dot{R}} (t) \mathbf{R} {(t)^T} \right)^T \] 可见\(\mathbf{\dot{R}} (t) \mathbf{R} {(t)^T}\)是一个反对称矩阵，将其记作\(\mathbf{A}\)，于是\(\mathbf{A}^T=-\mathbf{A}\),所以它主对角线元素必为，而非对角线元素则只有三个自由度。我们一定可以找到一个这样的向量\(\mathbf{a}=[a_1, a_2, a_3]^T\)使得： \[ {\mathbf{a}^ \wedge } = \mathbf{A} = \left[ {\begin{array}{*{20}{c}} 0& -a_3 & a_2\\ {a_3}&0& - {a_1}\\ - {a_2}&{a_1}&0 \end{array}} \right] \]

其中\(^{\wedge}\)符号表示由向量转换为矩阵，反之我们也可以用\(^{\vee}\)符号定义由矩阵转换为向量的方式: \[ { \mathbf{A}^ \vee } = \mathbf{a} \]

现在，由于\(\mathbf{\dot{R}} (t) \mathbf{R} {(t)^T}\)是一个反对称矩阵，所以我们一定可以找到一个三维向量\(\mathbf{\phi} (t) \in \mathbb{R}^3\)与之对应。于是我们有： \[\mathbf{ \dot{R} } (t) \mathbf{R}(t)^T = \mathbf{\phi} (t) ^ {\wedge}\] 左右各右乘\(\mathbf{R}(t)\)，由于其正交性，有： \[ \mathbf{ \dot{R} } (t) = \mathbf{\phi} (t)^{\wedge} \mathbf{R}(t) = \left[ {\begin{array}{*{20}{c}} 0&- {\phi _3}&{\phi _2}\\ {\phi _3}&0& - {\phi _1}\\ { - \phi _2}&{\phi _1}&0 \end{array}} \right] \mathbf{R} (t) \]

可以看到，每对旋转矩阵求一次导数，只需左乘一个矩阵\(\mathbf{\phi} (t)^{\wedge}\)即可。由于\(\mathbf{\phi} (t)^{\wedge}\)反映了的导数性质，故称它在的正切空间(tangent space)上。同时在\(t_0\)附近，设\(\mathbf{\phi}\)保持为常数\(\mathbf{\phi}(t_0)=\mathbf{\phi}_0\)，我们有： \[ \mathbf{ \dot{R} } (t) = \mathbf{\phi} (t_0)^{\wedge} \mathbf{R}(t)= \mathbf{\phi}_0^{\wedge} \mathbf{R}(t) \] 又因为初始值$ (0) = $对上式进行求解可得：

\[\label{eq:so3ode} \mathbf{R}(t) = \exp \left( \mathbf{\phi}_0^{\wedge} t\right) .\]

上式描述\(\mathbf{R}\)在局部的导数关系。

李代数 \(\mathfrak{so}(3)\)

上文提及的\(\mathbf{\phi}\)是一种李代数，\(SO(3)\)对应的李代数是定义在\(\mathbb{R}^3\)上的向量，我们记作\(\mathbf{\phi}\)，它 对应与一个反对称矩阵：

\[ \label{eq:phi} \mathbf{\Phi} = \mathbf{\phi}^{\wedge} = \left[ {\begin{array}{*{20}{c}} 0&{ - \phi _3}&{\phi _2}\\ {\phi _3}&0&{ - \phi _1}\\ { - \phi _2}&{\phi _1}&0 \end{array}} \right] \in \mathbb{R}^{3 \times 3} \]

由于它与反对称矩阵关系很紧密，在不引起歧义的情况下，就说的元素是3维向量或者3维反对称矩阵，不加区别： \[\bbox[5px,border:2px solid red] { \mathfrak{so}(3) = \left\{ \Phi = \mathbf{\phi^\wedge} \in \mathbb{R}^{3 \times 3} | \mathbf{\phi} \in \mathbb{R}^3 \right\} } \]

李代数 \(\mathfrak{se}(3)\)

\(SE(3)\)对应的李代数为\(\mathfrak{se}(3)\)，\(\mathfrak{se}(3)\)定义在\(\mathbb{R}^{6}\)空间，其具体形式如下：

\[\bbox[5px,border:2px solid red] { \mathfrak{se}(3) = \left\{ \mathbf{ \xi } = \left[ \begin{array}{l} \mathbf{\rho} \\ \mathbf{\phi} \end{array} \right] \in \mathbb{R}^{6}, \mathbf{\rho} \in \mathbb{R}^{3},\mathbf{\phi} \in \mathfrak{so}(3),\mathbf{\xi}^\wedge = \left[ {\begin{array}{*{20}{c}} \mathbf{\phi} ^ \wedge &\mathbf{\rho} \\ \mathbf{0}^T&0 \end{array}} \right] \in \mathbb{R}^{4 \times 4} \right\} } \]

\(\mathfrak{se}(3)\)是一个这样的六维向量，前三维表示平移，记作\(\mathbf{\rho}\)；后三维表示旋转，记作\(\mathbf{\phi}\)（有时候这两个参数会反过来，可也可以的）。

指数映射

\(\mathfrak{so}(3)\)以及\(\mathfrak{se}(3)\)的指数映射分别对应于\(SO(3)\)以及\(SE(3)\)，它们之间的转换关系可以由下图表示：

lieGroup

李代数求导

对旋转矩阵李代数求导

对\(\mathbf{R}\)进行一次扰动\(\Delta \mathbf{R}\)，假设左扰动\(\Delta \mathbf{R}\)对应的李代数为$ {}\(，对\) {}$求导，得到：

\[ \begin{aligned} \frac{\partial ({\boldsymbol Rp})}{\partial {\boldsymbol \varphi}} &= \lim_{\boldsymbol \varphi \to 0} \frac{ \overbrace{ \exp ({\boldsymbol \varphi}^{\land}) }^{\color{Red}{可作泰勒展开}} \exp ({\boldsymbol \phi}^{\land}) {\boldsymbol p} - \exp ({\boldsymbol \phi}^{\land}) {\boldsymbol p}} { {\boldsymbol \varphi} }\\ &\approx \lim_{\boldsymbol \varphi \to 0} \frac{({\boldsymbol I} + {\boldsymbol \varphi}^{\land}) \exp ({\boldsymbol \phi}^{\land}) {\boldsymbol p} - \exp ({\boldsymbol \phi}^{\land}) {\boldsymbol p}} { {\boldsymbol \varphi} } \\ &= \lim_{\boldsymbol \varphi \to 0} \frac{ {\boldsymbol \varphi}^{\land} {\boldsymbol {Rp}} } { {\boldsymbol \varphi} } \\ &= \lim_{\boldsymbol \varphi \to 0} \frac{ -({\boldsymbol {Rp}})^{\land} {\boldsymbol \varphi} } { {\boldsymbol \varphi} } \\ &= -({\boldsymbol {Rp}})^{\land} \end{aligned} \]

变换转矩阵李代数求导

假设空间点\({\boldsymbol p}\)经过一次变换\({\boldsymbol T}\)（对应的李代数为\({\boldsymbol \xi}\)）后变为 \({\boldsymbol Tp}\) 。当给\({\boldsymbol T}\)左乘一个扰动\(\Delta {\boldsymbol T} = \exp (\delta {\boldsymbol \xi}^{\land})\)，设扰动项的李代数为\(\delta {\boldsymbol \xi} = [\delta {\boldsymbol \rho}, \delta {\boldsymbol \phi}]^{T}\)，有：

\[ \begin{aligned} \frac{\partial ({\boldsymbol {Tp}})}{\partial \delta{\boldsymbol \xi}} &= \lim_{\delta{\boldsymbol \xi} \to 0} \frac{ \overbrace{ \exp (\delta {\boldsymbol \xi}^{\land}) }^{\color{Red}{可作泰勒展开}} \exp ({\boldsymbol \xi}^{\land}) {\boldsymbol p} - \exp ({\boldsymbol \xi}^{\land}) {\boldsymbol p}} { \delta {\boldsymbol \xi} } \\ &\approx \lim_{\delta{\boldsymbol \xi} \to 0} \frac{ ({\boldsymbol I} + \delta {\boldsymbol \xi}^{\land}) \exp ({\boldsymbol \xi}^{\land}) {\boldsymbol p} - \exp ({\boldsymbol \xi}^{\land}) {\boldsymbol p} } { \delta {\boldsymbol \xi} } \\ &= \lim_{\delta{\boldsymbol \xi} \to 0} \frac{ \delta {\boldsymbol \xi}^{\land} \exp ({\boldsymbol \xi}^{\land}) {\boldsymbol p} } { \delta {\boldsymbol \xi} } \\ &= \lim_{\delta{\boldsymbol \xi} \to 0} \frac{ \begin{bmatrix} \delta {\boldsymbol \phi}^{\land} & \delta {\boldsymbol \rho} \\ {\boldsymbol 0}^{T} & 1 \\ \end{bmatrix} \begin{bmatrix} {\boldsymbol {Rp}} + {\boldsymbol t} \\ 1 \\ \end{bmatrix} } { \delta {\boldsymbol \xi} } \\ &= \lim_{\delta{\boldsymbol \xi} \to 0} \frac{ \begin{bmatrix} \delta {\boldsymbol \phi}^{\land} ({\boldsymbol {Rp}} + {\boldsymbol t}) + \delta {\boldsymbol \rho} \\ 0 \\ \end{bmatrix} } { \delta {\boldsymbol \xi} } \\ &= \overbrace{ \begin{bmatrix} {\boldsymbol I} & -({\boldsymbol {Rp}} + {\boldsymbol t})^{\land} \\ {\boldsymbol 0}^{T} & {\boldsymbol 0}^{T} \\ \end{bmatrix} }^{\color{Red}{上式分块求导}}\\ &= ({\boldsymbol {Tp}})^{\bigodot} \end{aligned} \]

上式中运算符号\(\bigodot\)的含义：将一个齐次坐标的空间点变换成一个\(4 \times 6\)的矩阵。

补充

\(SE(3)\)左扰

\[ \begin{aligned} \rm{exp}\left( {\Delta {\xi}^{\land} } \right){\rm{exp}}\left( {\xi}^{\land} \right) & \approx \left( {I + {\left[ {\Delta \xi } \right]}_ \times } \right){\rm{exp}}(\xi) \\ &= \left( I_{4 \times 4} + \left[ \begin{array}{*{20}{c}} { \left[ \Delta \phi \right]}_{\times} & \Delta \rho \\ 0&0 \end{array} \right] \right) \left[ \begin{array}{*{20}{c}} R & t \\ 0&1 \end{array} \right] \\ &= \left[ \begin{array}{*{20}{c}} { \left[ \Delta \phi \right]}_{\times}+I_{3 \times 3} & \Delta \rho \\ 0&1 \end{array} \right] \left[ \begin{array}{*{20}{c}} R & t \\ 0&1 \end{array} \right] \\ &= \left[ \begin{array}{*{20}{c}} \left( \left[ \Delta \phi \right]_{\times}+I_{3 \times 3} \right) R & \left( \left[ \Delta \phi \right]_{\times}+I_{3 \times 3} \right) t + \Delta \rho \\ 0&1 \end{array} \right] \end{aligned} \]

\(SE(3)\)右扰

\[ \begin{aligned} \rm{exp}\left( {\xi}^{\land} \right) \rm{exp}\left( {\Delta {\xi}^{\land} } \right) & \approx {\rm{exp}}({\xi}^{\land}) \left( {I + {\left[ {\Delta \xi } \right]}_ \times } \right)\\ &= \left[ \begin{array}{*{20}{c}} R & t \\ 0&1 \end{array} \right] \left( I_{4 \times 4} + \left[ \begin{array}{*{20}{c}} { \left[ \Delta \phi \right]}_{\times} & \Delta \rho \\ 0&0 \end{array} \right] \right)\\ &= \left[ \begin{array}{*{20}{c}} R & t \\ 0&1 \end{array} \right] \left[ \begin{array}{*{20}{c}} { \left[ \Delta \phi \right]}_{\times}+I_{3 \times 3} & \Delta \rho \\ 0&1 \end{array} \right] \\ &= \left[ \begin{array}{*{20}{c}} R \left( \left[ \Delta \phi \right]_{\times}+I_{3 \times 3} \right) & R \Delta \rho + t \\ 0&1 \end{array} \right] \end{aligned} \]