视觉SLAM十四讲笔记-4-2

视觉SLAM十四讲笔记-4-2

4.3 李代数求导与扰动模型

4.3.1 BCH公式与近似形式

使用李代数形式的一大动机是进行优化，而在优化过程中导数是非常必要的信息。
在上一节中已经知道了 $SO(3)$ 和 $SE(3)$ 上李群和李代数关系，但是，当在 $SO(3)$ 中完成两个矩阵乘法时，李代数中 $so(3)$ 发生了什么改变尼？
当 $so(3)$ 上做两个李代数的加法时， $SO(3)$上是否对应着两个矩阵的乘积?
即：
$$\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)=\exp \left({\left({\varphi }_1+{\varphi }_2\right)}^{\wedge }\right)?$$
很遗憾，矩阵的指数函数的乘积不等于矩阵加法后的指数函数。上式不成立。
两个李代数指数映射乘积的完整形式，由 Baker-Campbell Hausdorff (BCH) 公式给出。由于其完整形式太过复杂，这里只给出其展开形式的前几项：
$$\ln (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{2}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\cdots$$
其中 [] 为李括号。从 BCH 可以看出，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。特别地，考虑 $SO(3)$ 上的李代数 $\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }$，当 ${\varphi }_1$ 或者 ${\varphi }_2$ 为小量时，小量二次以上的项都可以忽略。此时，BCH 拥有线性近似表达：
$$\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }\approx \{\begin{array}{ll}{\mathbf{J}}_l{\left({\varphi }_2\right)}^{-1}{\varphi }_1+{\varphi }_2& \text{ if }{\varphi }_1\text{ is small }\\ {\mathbf{J}}_r{\left({\varphi }_1\right)}^{-1}{\varphi }_2+{\varphi }_1& \text{ if }{\varphi }_2\text{ is small. }\end{array}$$
以第一个近似为例。当对一个旋转矩阵 $R_2$ (李代数为 ${\varphi }_2$) 左乘一个微小的旋转矩阵 $R_1$ (李代数为 ${\varphi }_1$) 时，可以近似地看作，在原有的李代数 ${\varphi }_2$ 上加上了一项 $J_1({\varphi }_2{)}^{-1}{\varphi }_1$。同理，第二个近似描述了右乘一个微小位移的情况。于是，李代数在 BCH 近似下，分成了左乘和右乘近似两种，在使用时必须注意使用的是左乘模型和右乘模型。
本书以左乘为例，左乘 BCH 近似雅克比 $J_l$ 事实上就是就是：
$${\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 }$$
它的逆为：
$${\mathbf{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)\mathbf{a}{\mathbf{a}}^{\mathbf{T}}-\frac{\theta }{2}{\mathbf{a}}^{\wedge }$$
而右乘雅克比仅需要对自变量取负号即可：
$${\mathbf{J}}_r(\varphi )={\mathbf{J}}_l(-\varphi )$$
这样就可以谈论李群乘法与李代数加法的关系了。
为了方便理解，重新叙述一下 BCH 近似的意义。假定对某个旋转 $R$，对应的李代数为 $\varphi$，给它左乘一个微小旋转，记为 $\Delta R$，对应的李代数为 $\Delta \varphi$。那么在李群上得到的结果就是 $\Delta R.R$，而在李代数上，根据 BCH 近似，为 $J_l^{-1}(\varphi )\Delta \varphi +\varphi$。
$$\exp \left(\Delta {\varphi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)=\exp \left({\left(\varphi +J_l^{-1}(\varphi )\Delta \varphi \right)|}^{\wedge }\right)$$
反之，如果在李代数上进行加法，让一个 $\varphi$ 加上一个 $\Delta \varphi$，那么可以近似为李群上带左右雅克比的乘法：
$$\exp \left((\mathbf{\varphi }+\Delta \mathbf{\varphi }{)}^{\wedge }\right)=\exp \left({\left({\mathbf{J}}_l\Delta \mathbf{\varphi }\right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)=\exp \left({\mathbf{\varphi }}^{\wedge }\right)\exp \left({\left({\mathbf{J}}_r\Delta \mathbf{\varphi }\right)}^{\wedge }\right)$$
这样就为之后李代数上做微积分提供了理论基础。同样地，对于 $SE(3)$，也有类似的 BCH 近似：
$$\begin{array}{rl}& \exp \left(\Delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_l^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }\right)}^{\wedge }\right)\\ & \exp \left({\mathbf{\xi }}^{\wedge }\right)\exp \left(\Delta {\mathbf{\xi }}^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_r^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }\right)}^{\wedge }\right)\end{array}$$
这里 ${\mathcal{J}}_l$ 形式比较复杂，它是一个 6 * 6 的矩阵。这里先不介绍其具体形式。

4.3.2 SO(3)上的李代数求导

根据一个实际问题讨论一个带有李代数的函数以及关于该李代数求导的问题。
在 SLAM 中，要估计一个相机的位置和姿态，该位姿是由 $SO(3)$ 上的旋转矩阵或者 $SE(3)$ 上的变换矩阵来描述的。不妨设某个时刻机器人的位姿为 $T$。它观察到了一个世界坐标系位于 $p$ 的点，产生了一个观测数据 $z$。那么，由坐标变换得：
$$z=Tp+w$$
$w$ 为随机噪声。由于它的存在， $z$ 往往不能精确地满足 $z=Tp$ 的关系。所以，通常会计算理想的观测与实际数据的误差：(理想的观测是 $Tp$，而实际的观测为 $z$)
$$e=z-Tp$$
假设一共有 $N$ 个观测点，于是就有 $N$ 个上面的式子。那么，对于机器人进行位姿估计，相当于寻找一个最优的 $T$，使得整体误差最小化：
$$mi{n}_{T}J(T)=\sum _{i=1}^N{\Vert z_i-T{p}_i\Vert }_2^2$$
而求解此问题，需要计算目标函数 $ J(T)$ 关于变换矩阵 $T$ 的导数。 具体的算法留到后面再讲。这里的重点是， 通常会构建与位姿相关的函数，然后讨论该函数关于位姿的导数，一调整当前的估计值。
然而，$SO(3)$ 或者 $SE(3)$ 上并没有良好定义的加法，它们只是群。如果把 $T$ 当成一个普通矩阵来处理优化，就必须加以约束。而从李代数角度来说，由于李代数由向量构成，具有良好的加法运算，因此，使用李代数解决求导问题的思路分为两种：
1.用李代数表示姿态，然后根据李代数加法对李代数就行求导；
2.对李群左乘或者右乘微小扰动，然后对该扰动就行求导，称为左扰动或右扰动模型。
第一种形式对应李代数的求导模型，而第二种则对应到扰动模型。

4.3.3 李代数求导

首先，考虑 $SO(3)$ 上的情况，假设对于一个空间点 $p$ 进行了旋转，得到了 $Rp$。现在要计算旋转之后点的坐标相对于旋转的导数，可以记为:
$$\frac{\partial (Rp)}{\partial R}$$
由于 $SO(3)$ 没有加法，所以该导数无法按照导数的定义进行计算。设 $R$ 对应的李代数为 $\varphi$，转而计算：
$$\frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \varphi }$$
按照导数的定义，有：
$$\begin{array}{rl}\frac{\partial \left(\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}\right)}{\partial \mathbf{\varphi }}& =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\exp \left((\mathbf{\varphi }+\delta \mathbf{\varphi }{)}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\exp \left({\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & \approx \underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\left(\mathbf{I}+{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{-{\left(\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}\right)}^{\wedge }{\mathbf{J}}_l\delta \mathbf{\varphi }}{\delta \mathbf{\varphi }}=-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\end{array}$$
其中，第 2 行为 4.3.1 中 BCH 线性近似，第 3 行为泰勒展开舍去高阶项后的近似(由于取了极限，可以写等号)，第 4 行到第 5 行将反对称符号看做叉积，交换之后变号。于是，推导出旋转之后的点相对于李代数的导数：
$$\frac{\partial (Rp)}{\partial \varphi }=(-Rp{)}^{\wedge }{J}_l$$
这里求导中仍然包含形式比较复杂的 $J_l$，而计算它太麻烦，因此不希望计算它。下面的扰动模型则提供了更简单的导数计算公式。

4.3.4 扰动模型(左乘)

另外一种求导方式是对 $R$ 进行一次扰动 $\Delta R$，看结果相对于扰动的变化率。这个扰动可以左乘或者右乘，最后结果会有一点微小的差异。这里以左乘为例，设左扰动 $\Delta R$ 对应的李代数为 $\phi$ 。然后对 $\phi$ 求导，即：
$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\phi }}& =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & \approx \underset{\mathbf{\phi }\to 0}{\lim }\frac{\left(1+{\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{{\mathbf{\phi }}^{\wedge }\mathbf{R}\mathbf{p}}{\phi }=\underset{\mathbf{\phi }\to 0}{\lim }\frac{-(\mathbf{R}\mathbf{p}{)}^{\wedge }\mathbf{\phi }}{\phi }=-(\mathbf{R}\mathbf{p}{)}^{\wedge }\end{array}$$
可见，相比于直接对李代数求导，省去了一个雅克比矩阵 $J$ 的计算。这使得扰动模型更为实用。这里的求导运算很重要，在位姿估计中具有重要的意义。

4.3.5 SE(3)上的李代数求导

$SE(3)$ 上的李代数求导就不再介绍了。这里直接给出$SE(3)$ 上的扰动模型。
假设某空间点 $p$ 经过一次变换 $T$(对应的李代数为 $\xi$)，得到 $Tp$。现在给 $T$ 左乘一个扰动 $\Delta T=exp(\delta {\xi }^{\wedge })$。设扰动项的李代数为 $\delta \xi =[\delta \rho ,\delta \varphi {]}^T$，那么：
$$\begin{array}{rl}& \frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \mathbf{\xi }}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & \approx \underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\delta {\mathbf{\xi }}^{\wedge }\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \xi \to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\mathbf{p}+\mathbf{t}\\ 1\end{array}\right]}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}\mathbf{p}+\mathbf{t})+\delta \mathbf{\rho }\\ 0\end{array}\right]}{\delta \mathbf{\xi }}=\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{R}\mathbf{p}+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\triangleq (\mathbf{T}\mathbf{p}{)}^{\odot }\end{array}$$
矩阵求导方面的顺序，假设 $a,b,x,y$　都是列向量，那么有如下规则：
$$\frac{d\left[\begin{array}{c}a\\ b\end{array}\right]}{d\left[\begin{array}{c}x\\ y\end{array}\right]}={\left[\frac{d[a,b{]}^T}{d\left[\begin{array}{c}x\\ y\end{array}\right]}\right]}^T={\left[\begin{array}{cc}\frac{da}{dx}& \frac{db}{dx}\\ \frac{da}{dy}& \frac{db}{dy}\end{array}\right]}^T=\left[\begin{array}{cc}\frac{da}{dx}& \frac{da}{dy}\\ \frac{db}{dx}& \frac{db}{dy}\end{array}\right]$$
至此，已经介绍了李群和李代数上的微分运算，在之后的章节中，将运用这些知识解决实际问题。