SLAM——李群和李代数
在SLAM中，我们需要对姿态进行估计和优化。但是旋转矩阵自身是有约束的，增加优化难度。因此我们需要引入李群和李代数，可以将位姿估计转换为无约束的优化问题。
群(Group)
群是一种集合加上一种运算的代数结构。如果记集合为\(A\)，运算为\(\cdot\),如果满足以下性质，则称\((A,\cdot)\)为群： 1. 封闭性： \(\forall a_1,a_2\in A,a_1 \cdot a_2 \in A\). 2. 结合律：\(\forall a_1,a_2,a_3 \in A (a_1 \cdot a_2) \cdot a_3 = a_1 \cdot (a_2 \cdot a_3)\). 3. 幺元：\(\exists a_0 \in A, s.t. \forall a \in A,a_0\cdot a = a \cdot a_0 = a\). 4. 逆：\(\forall a\in A,\exists a^{-1} \in A, a\cdot a^{-1} = a_0\).
可以验证旋转矩阵和转换矩阵与矩阵乘法运算都可以构成群。其中旋转矩阵与乘法构成的群为\(SO(3)\),特殊正交群，欧式转换矩阵与乘法构成特殊欧氏群\(SE(3)\). ### 李群（Lie Group） ###
具有连续（光滑）性质的群。李群既是群也是流形。SO(3),SE(3)都是李群，但是只有定义良好的乘法，没有加法，所以难以进行极限求导（\(+\Delta x\)）等操作。
引出李代数
一种李群，对应一个李代数。李代数用小写表示，如\(so(3),se(3)\).
任意旋转矩阵，则\(RR^T = I\).
考虑R随时间变化，有\(R(t)R(t)^T = I\).
两侧对t求导：\(R'(t)R(t)^T + R(t)R'(t)^T = 0\).
则：\(R'(t)R(t)^T = -(R'(t)^T R(t)^T)^T\).
所以我们可以知道：\(R'(t)R(t)^T\)是一个反对称矩阵。反对称矩阵都会有一个对应的向量，假设\(R'(t)R(t)^T\)对应的向量为\(\phi(t)\),则：
\(R'(t)R(t)^T = \phi(t)\)
两侧同时右乘\(R(t)\)，得到：
\(R'(t) = \phi(t)^{\hat{} } R(t)\)
所以我们可以看到对R求导之海，多出来一个\(\phi(t)\).
假如\(t_0=0,R(t_0) = I\)，对于\(R(t)\)进行泰勒展开： \[ R(t) \approx R(t_0) + R'(t_0)(t - t_0) = I+\phi(0)t. \]
在很短的时间(\(t=t_0+\Delta t\))里，假设\(\phi(t_0)\)不会变化。
\(R'(t) = \phi(t_0)^{\hat{} }R(t) = \phi_0^{\hat{} }R(t)\)
如果解上述微分方程（\(R(0) = I\)），可以得到：\(R(t) = exp(\phi_0^{\hat{} } t)\)
所以我们就得到了一个在\(t_0\)附近的近似估计。
实际上\(phi\)就是\(SO(3)\)对应的李代数。
李代数
李代数由一个集合\(\mathbb{V}\)，一个数域\(\mathbb{F}\),一个二元运算\([ ,]\)组成。这个运算某一定程度上反映了两个数的差异。李代数满足下面几个性质： 1. 封闭性：\(\forall X,Y \in \mathbb{V},[X,Y ] \in \mathbb{V}\) 2. 双线性：\(\forall X,Y,Z \in \mathbb{V},a,b \in \mathbb{F}\),有： \[ [aX+bY,Z ]= a[X,Z ] + b[Y,Z ],[Z,aX+bY ] = a[Z,X ]+b[Z,Y ]. \] 3. 自反性：\(\forall X \in \mathbb{V},[X, X] = 0\) 4. 雅科比等价：\(\forall X,Y,Z \in \mathbb{V},[X, [Y,Z ]] + [Z, [X,Y ]]+ [Y, [Z,X ]] = 0\)
其中二元运算被称为李括号。
李代数\(so(3)\) ###：
\(so(3) = \{\phi \in \mathbb{R}^3 ,\Phi = \phi ^{\hat{} } \in \mathbb{R}^{3 \times 3} \}\).
李括号：\([\phi_1,\phi_2 ]= (\Phi_1\Phi_2 - \Phi_2 \Phi_1)^{\hat{} }\).
Note: \(\Phi = \phi^{\hat{} },\phi = \Phi^{\hat{} }\).
李代数\(se(3)\)
\(se(3) = \left\{ \xi = \begin{bmatrix} \rho\\ \phi \end{bmatrix} \in \mathbb{R}^6,\rho \in \mathbb{R}^3,\phi \in so(3),\xi ^{\hat{} } = \begin{bmatrix} \phi^{\hat{} }&\rho\\ 0&0 \end{bmatrix} \right\}\)
指数与对数映射
之前引出李代数中推导的\(R(t) = exp(\phi_0t)\),R是李群SO(3)，而\(\phi\)是李代数so(3).他们之间是有一一对应的关系的。恩，从上面的推导中并不能保证R就是\(\phi\)的exp映射，但是实际上就是这样做的。
但是如何对一个\(\phi\)进行指数映射？映射还需转换为\(\Phi\).对于一个矩阵，我们是没法进行exp运算的。因此我们使用泰勒展开： \[ exp(A) = \sum_{n=0}^\infty \frac{1}{n!}A^n \]
同样上述展开也没那么容易进行，好在还有一些别的方法。
任何一个向量，我们都可以分解成方向和长度，也就是\(\theta \mathbf{a}\).其中\(\mathbf{a}\)为单位向量。
然后，我们可以验证的是：
\(\mathbf{a}^{\hat{} }\mathbf{a}^{\hat{} } = \mathbf{aa}^T -I\)
\(\mathbf{a}^{\hat{} }\mathbf{a}^{\hat{} }\mathbf{a}^{\hat{} } = -\mathbf{a}^{\hat{} }\)
因为我们\(\Phi = \phi^{\hat{} }\),所以上式提供了很好的办法来解决exp问题。经过推导，我们可以得到：
\[ \begin{aligned} exp(\phi^{\hat{} }) &= exp(\theta \mathbf{a}^T) = \sum_{n=0}^\infty \frac {1} {n!} (\theta \mathbf{a}^{\hat{} })^n\\ &=\cos \theta I + (1 - \cos \theta)\mathbf{aa}^T + \sin \theta \mathbf{a}^{\hat{} } \end{aligned} \]
很神奇的事情，上面竟然就是罗德里克斯公式（推导过程实际上不复杂，省略掉了）。
对应的还有一个对数映射：
\(\phi\) = (R)^{ } = (_{n=0}(R-I){n+1} )$
当然，既然我们已经知道罗德里克斯公式了，就不用上式这么复杂的式子去求对数映射了。
旋转角，和旋转矩阵不是一一对应的，因为旋转矩阵唯一的情况下，旋转角可能有多个（角度多了\(2\pi\)）。这意味着指数映射是满射的。如果角度限制到\([-\pi,\pi ]\)，那么李群和李代数元素就是一一对映的了。
同样，我们也可以得到\(SE(3)\)与\(se(3)\)的指数映射和对数映射: \[ \begin{aligned} exp(\xi^{\hat{} }) &= \begin{bmatrix} \sum_{n=0}^\infty \frac{1}{n!}(\phi ^{\hat{} })^n& \sum_{n=0}^\infty \frac{1}{(n+1)!}(\phi^{\hat{} })^n \rho\\ 0&1 \end{bmatrix} & \triangleq \begin{bmatrix} R&J\rho\\ 0&1 \end{bmatrix} = T \end{aligned} \]
上式中\(J = \frac{\sin \theta}{\theta}I + \left(1 - \frac {\sin\theta}{\theta}\right)\mathbf{aa}^T + \frac{1 - \cos \theta}{\theta} \mathbf{a}^{\hat{} }\)
它的对数映射，右上角与之前一样，而这里的\(t = J\rho\),通过\(\phi\)可以计算出来J，从而解线性方程得到\(\rho\).
J为雅克比矩阵。
李代数的求导与扰动模型
之所以要学习李群和李代数，是因为我们的SO(3)和SE(3)都是没有定义加法的。所以无法求导，对于优化非常难办。因此我们想做的是能否将李群和李代数上对应关系运用到求导中，把对李群的求导，转化为对李代数的求导。
所以我们希望的是：\(exp(\Phi_1 +\Phi_2) = exp(\Phi_1)exp(\Phi_2)\).
很遗憾的是，上面的式子不成立。（告辞）
BCH公式
有一个BCH公式，它是对于矩阵来说的以及李群李代数相关的指数相乘展开：
$ (exp(A)exp(B)) A+B+ 2 [A,B ]+ {12} [A, [A ,B]]+... $
可以看到，在数学上，它会有这么多的余项存在的。不过我们做近似的估计。如果\(\phi_1\)或者\(\phi_2\)有一个量是小量，我们可以忽略其二次项。直接考虑到李代数的转换：
$ (exp(_1^{ })exp(_2^{ }))^{ } { \[\begin{matrix} J_l(\phi_2)^{-1}\phi_1 +\phi_2&\phi_1为小量\\ J_r(\phi_1)^{-1}\phi_2 +\phi_1&\phi_2为小量\\ \end{matrix}\]. $
既然导数模型左或者右加上一个小量，上式中，第一个对应的是\(\phi_2\)为R对应的李代数，而\(\phi_1\)为小量，对应到李群，就是R左乘一个小量，第二个式子对应的就是右乘一个小量了。我们后面规定使用为左乘，实际上右乘的J和左乘也相差不多。
\[ J_l = J = \frac {\sin\theta}{\theta} I + (1 - \frac {\sin \theta}{\theta})\mathbf{aa}^T + \frac{1 - \cos \theta}{\theta} \mathbf{a}^{\hat{} } \]
\[ J_l^{-1} = \frac{\theta}{2} \cot \frac{\theta}{2}I + (1 - \frac{\theta}{2}\cot\frac{\theta}{2})\mathbf{aa}^T - \frac{\theta}{2}\mathbf{a}^{\hat{} } \]
右乘的雅科比矩阵：\(J_r (\phi) = J_l(-\phi)\).
也就是：\(\exp(\Delta \phi ^{\hat{} })\exp(\phi^{\hat{} }) \approx \exp ((\phi + J_l^{-1}(\phi)\Delta \phi)^{\hat{} })\).
同理，如果我们在李代数上进行加法： \[ \exp((\phi+\Delta \phi)^{\hat{} }) = \exp((J\Delta \phi)^{\hat{} })\exp(\phi^{\hat{} }) \] 上述都是左乘模型。
同样的，对于SE(3)也有类似的BCH近似公式： \[ \exp(\Delta \xi ^{\hat{} }) \exp(\xi^{\hat{} }) \approx \exp((\mathcal{J}_l^{-1}\Delta \xi + \xi)^{\hat{} }),\\ \exp(\xi^{\hat{} })\exp(\Delta \xi ^{\hat{} }) \approx \exp((\mathcal{J}_r^{-1}\Delta \xi + \xi)^{\hat{} }). \]
\(\mathcal{J}_l\)的形式比较复杂，我们也用不上，就不提了。
so(3)求导
我们想要实现对旋转矩阵的求导，由于旋转矩阵是乘法定义的，所以直接在SO(3)上无法定义出来导数。因此要转换到对应的李代数上来求得导数。
为了得到so(3)上的导数，有两种办法。第一种，是通过so(3)来实现，另一种是给SO(3)左乘一个矩阵来实现。
导数模型
根据导数的定义： \[ \begin{aligned} \frac{\partial (\exp(\phi^{\hat{} })p)} {\partial \phi}&= \lim_{\delta \phi \rightarrow 0} \frac{\exp((\phi + \delta \phi)^{\hat{} })p - exp(\phi^{\hat{} })p} {\delta \phi}\\ &= \lim_{\delta \phi \rightarrow 0} \frac{exp((J_l\delta \phi)^{\hat{} })exp(\phi^{\hat{} })p - \exp(\phi^{\hat{} })p}{\delta \phi}\\ &\approx \lim_{\delta \phi \rightarrow 0} \frac{(I+(J_l\delta \phi)^{\hat{} })exp(\phi^{\hat{} })p - \exp(\phi^{\hat{} })p}{\delta \phi}\\ &= \lim_{\delta \phi \rightarrow 0} \frac{(J_l\delta\phi)^{\hat{} }\exp(\phi^{\hat{} })p}{\delta \phi}\\ &= \lim_{\delta \phi \rightarrow 0} \frac{-(\exp(\phi^{\hat{} })p)^{\hat{} }J_l\delta\phi}{\delta \phi}\\ &= -(Rp)^{\hat{} } J_l \end{aligned} \] 上面的过程用到了BCH展开，泰勒近似，以及\(\hat{}\)的性质：$a^{ }b = -b^{ }a \(. 这里有形式比较复杂的\)J_l$,我们不想计算它。所以看看扰动模型。
扰动模型
扰动模型是在李群上左乘一个很小的矩阵\(\Delta R\),假设它对应的李代数为\(\theta\)。通过扰动模型得到的导数为\(-(Rp)^{\hat{} }\). \[\begin{aligned} \frac{\partial (Rp)} {\partial \phi}&= \lim_{\theta \rightarrow 0} \frac{\exp(\theta^{\hat{} })\exp(\phi)^{\hat{} })p - exp(\phi^{\hat{} })p} {\theta}\\ &=-(Rp)^{\hat{} } \end{aligned}\]扰动模型求导和上面导数模型可以使用同样的套路，而且更简单。因此就不详细写出来了。
可以看到扰动模型的比导数模型得到的结果更加简便一些。因此扰动模型相对于导数模型来说更加的实用。
se(3)求导
对于se(3)的求导，我们直接给出扰动模型： \[ \begin{aligned} \frac{\partial (Tp)}{\partial \delta \xi} &= \lim_{\delta \xi \rightarrow 0} \frac{\begin{bmatrix} \delta \phi^{\hat{} }(Rp+t) + \delta p\\ 0 \end{bmatrix} }{\delta \xi}\\ &= \begin{bmatrix} I&-(Rp+t)^{\hat{} }\\ 0&0 \end{bmatrix}\\ &\triangleq (Tp)^{\bigodot} \end{aligned} \]
如果没有李群和李代数的提出，求导就没有理论依据了。而因为有了李群和李代数这种映射关系，我们可以通过将李群用李代数来表示，而李代数是可以进行求导的。从而实现对旋转矩阵的求导。
Sophus库
李群李代数听得人头大。不过，我们不用自己去完成这些东西。对于李群李代数支持比较好的库是sophus，我们就使用这个库来实现SLAM中李群李代数需要的应用。
sophus是eigen的一个扩展，它在eigen的基础上实现了一些李群李代数的操作，没有任何别的依赖项。
1 |
|