[视觉SLAM十四讲]学习笔记6-李代数求导与扰动模型[左乘]

1 误差优化引出李代数求导问题

在SLAM的误差优化问题中，我们需要用到李代数求导。例如，我们需要估计一个相机的位置和姿态，该位姿是由$SO(3)$上的旋转矩阵或者$SE(3)$上的变换矩阵描述的。我们可以设某个时刻机器人的的位姿为$\mathbf{T}$。他观察到了一个世界坐标位于$\mathbf{p}$的点，产生了一个观测数据$\mathbf{z}$。由坐标变换关系可知：$$\mathbf{z}=\mathbf{Tp}+\mathbf{w}\text{\hspace{1em}(1.1)}$$这里$\mathbf{w}$为随机噪声，由于噪声的存在，观测数据$\mathbf{z}$一般不可能精确的满足$\mathbf{z}=\mathbf{Tp}$的关系。所以我们需要设置一个准确的观测与实际数据的误差$\mathbf{e}$：$$\mathbf{e}=\mathbf{z}-\mathbf{Tp}\text{\hspace{1em}(1.2)}$$我们可以假设共有$N$个这样的路标点和观测，那么就会有$N$个上述的关系式。对机器人进行位姿估计，等价于寻找一个最优的$\mathbf{T}$，使整体误差最小，即满足下面的式子：$$\underset{\mathbf{T}}{\min }J(\mathbf{T})=\sum _{i=1}^N\Vert z_i-{\mathbf{Tp}}_i{\Vert }_2^2\text{\hspace{1em}(1.3)}$$若想求解此问题，我们需要计算目标函数$J$关于变换矩阵$\mathbf{T}$的导数。我们会经常构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。不过，$SO(3),SE(3)$上没有合适定义的加法，他们只是群。若我们把变换矩阵$\mathbf{T}$当成一个普通矩阵来处理优化， 就必须对它加以约束。不过从李代数的角度来讲，由于李代数是由向量组成，有良好的加法运算，因此可以使用李代数解决求导问题，并且有两种思路如下：

• 用李代数表示姿态，然后根据李代数加法对李代数求导。
• 对李群左乘或右乘微小扰动，然后对该扰动求导，这被称为左扰动和右扰动模型。
  这时候，解决方法就分为第一种"李代数的求导模型"和第二种"扰动模型"。
  在介绍求导之前，我们先介绍一下我们在求导过程中需要用到的BCH公式与近似公式

2 BCH公式与近似公式

2.1 BCH公式

上节我们说到，误差优化问题可以通过李代数求导解决。我们已经清楚了$SO(3)$和$SE(3)$上李群和李代数的关系，但是当我们在$SO(3)$中完成两个矩阵乘法时，李代数中$\mathfrak{so}(3)$上会发生什么改变，反之，当$\mathfrak{so}(3)$上做两个李代数的加法时，$SO(3)$上是否对应着两个矩阵的乘积？若成立，则形式如下：$$\exp ({\mathbf{\varphi }}_1^{\wedge })\exp ({\mathbf{\varphi }}_2^{\wedge })=\exp (({\mathbf{\varphi }}_1+{\mathbf{\varphi }}_2{)}^{\wedge }).\text{\hspace{1em}(2.1)}$$若${\mathbf{\varphi }}_1,{\mathbf{\varphi }}_2$为标量，显然结果成立；但我们计算的是矩阵的指数函数，而不是标量的指数。换一个说法，我们在研究下面的式子是否成立：$$\ln (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}?\text{\hspace{1em}(2.2)}$$显然，上述关系是在矩阵时并不成立。两个李代数指数映射乘积的完整形式，由$\mathbf{Baker}–\mathbf{Campbell}–\mathbf{Hausdorff}$公式(BCH公式)给出。维基百科给出的形式如下：
数学中$$e^X{e}^Y=e^Z\text{\hspace{1em}(2.3)}$$李群李代数中，$$Z=X+Y+\frac{1}{2}[X,Y]+\frac{1}{12}[X,[X,Y]]-\frac{1}{12}[Y,[X,Y]]+\cdots \text{\hspace{1em}(2.4)}$$本书中的形式为$$\ln (\exp (A)\exp (B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+\cdots \text{\hspace{1em}(2.5)}$$这里的[ ]为李括号。BCH公式的形式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。

2.2 BCH线性近似表达

当我们考虑$SO(3)$上的李代数$\ln (\exp ({\mathbf{\varphi }}_1^{\wedge })\exp ({\mathbf{\varphi }}_2^{\wedge }){)}^{\vee }$，当${\mathbf{\varphi }}_1$或${\mathbf{\varphi }}_2$为微小量时，微小量二次以上的项都可以被忽略。此时，BCH有线性近似表达形式如下：$$\ln (\exp ({\mathbf{\varphi }}_1^{\wedge })\exp ({\mathbf{\varphi }}_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{l}{\mathbf{J}}_l({\mathbf{\varphi }}_2{)}^{-1}{\mathbf{\varphi }}_1+{\mathbf{\varphi }}_2当{\mathbf{\varphi }}_1为微小量\\ {\mathbf{J}}_r({\mathbf{\varphi }}_1{)}^{-1}{\mathbf{\varphi }}_2+{\mathbf{\varphi }}_1当{\mathbf{\varphi }}_2为微小量\end{array}\text{\hspace{1em}(2.6)}$$对于上述关系式，针对第一种情况，我们可以这样理解，当对一个旋转矩阵${\mathbf{R}}_2$(李代数为${\mathbf{\varphi }}_2$)左乘一个微小旋转矩阵${\mathbf{R}}_1$(李代数为${\mathbf{\varphi }}_1$)时，可以近似的看作，在原有的李代数${\mathbf{\varphi }}_2$基础上加上了一项${\mathbf{J}}_l({\mathbf{\varphi }}_2{)}^{-1}{\mathbf{\varphi }}_1$。同理，第二种情况的近似描述了右乘一个微小位移的情况。这时候的李代数在BCH近似下，分成了左乘近似和右乘近似两种，使用时需分清是左乘模型还是右乘模型。
SLAM十四讲以左乘为例。这里左乘BCH近似雅可比${\mathbf{J}}_l$实际上是李群与李代数基础及指数/对数映射中的式(3.15)：$${\mathbf{J}}_l=\mathbf{J}=\frac{sin\theta }{\theta }\mathbf{I}+(1-\frac{sin\theta }{\theta }){\mathbf{aa}}^T+\frac{1-cos\theta }{\theta }{\mathbf{a}}^{\wedge }\text{\hspace{1em}(2.7)}$$取逆为：$${\mathbf{J}}_l^{-1}=\frac{\theta }{2}cot\frac{\theta }{2}\mathbf{I}+(1-\frac{\theta }{2}cot\frac{\theta }{2}){\mathbf{aa}}^T-\frac{\theta }{2}{\mathbf{a}}^{\wedge }.\text{\hspace{1em}(2.8)}$$右乘模型的雅可比仅需要对自变量取负号即可：$${\mathbf{J}}_r(\mathbf{\varphi })={\mathbf{J}}_l(-\mathbf{\varphi }).\text{\hspace{1em}(2.9)}$$针对这样的形式，我们就可以谈论李群乘法与李代数加法的关系了。

2.3 BCH近似意义

这里我们再次重新叙述一下BCH近似的意义。我们假设某个旋转$\mathbf{R}$，相应的李代数为$\mathbf{\varphi }$。我们对他左乘一个微小旋转，记作$\Delta \mathbf{R}\cdot \mathbf{R}$，在李代数上，根据BCH近似，为${\mathbf{J}}_l(\mathbf{\varphi }{)}^{-1}\mathbf{\Delta }\mathbf{\varphi }+\mathbf{\varphi }$。完整形式如下：$$\exp (\Delta {\mathbf{\varphi }}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })=\exp (({\mathbf{J}}_l(\mathbf{\varphi }{)}^{-1}\mathbf{\Delta }\mathbf{\varphi }+\mathbf{\varphi }{)}^{\wedge }).\text{\hspace{1em}(2.10)}$$反之，我们在李代数上进行加法，让$\mathbf{\varphi }$加上$\Delta \mathbf{\varphi }$，那么可以近似为利群上带左右雅可比的乘法：$$\exp ((\mathbf{\varphi }+\Delta \mathbf{\varphi }{)}^{\wedge })=\exp (({\mathbf{J}}_l(\mathbf{\varphi })\Delta \mathbf{\varphi }{)}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })=\exp ({\mathbf{\varphi }}^{\wedge })\exp (({\mathbf{J}}_r(\mathbf{\varphi })\Delta \mathbf{\varphi }{)}^{\wedge }).\text{\hspace{1em}(2.11)}$$这里做一下简单的推导 令$\Delta \Phi ={\mathbf{J}}_l(\mathbf{\varphi }{)}^{-1}\mathbf{\Delta }\mathbf{\varphi },\Phi =\mathbf{\varphi }$，这里$\Delta \mathbf{\varphi }={\mathbf{J}}_l(\mathbf{\varphi })\Delta \Phi$,可以得到：$$\exp ((\Phi +\Delta \Phi {)}^{\wedge })=\exp (({\mathbf{J}}_l(\mathbf{\varphi })\Delta \Phi {)}^{\wedge })\exp ({\Phi }^{\wedge }).\text{\hspace{1em}(2.12)}$$ $$\exp ((\mathbf{\varphi }+\Delta \mathbf{\varphi }{)}^{\wedge })=\exp (({\mathbf{J}}_l(\mathbf{\varphi })\Delta \mathbf{\varphi }{)}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })=\exp ({\mathbf{\varphi }}^{\wedge })\exp (({\mathbf{J}}_r(\mathbf{\varphi })\Delta \mathbf{\varphi }{)}^{\wedge }).\text{\hspace{1em}(2.13)}$$上述等式后两项的变形参考式()和指对数性质这样的形式为之后在李代数上作微积分提供了理论基础。同样，对于$SE(3)$，也有类似的BCH近似，形式入下：$$\{\begin{array}{l}\exp (\Delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })\approx \exp (({\mathcal{J}}_l(\mathbf{\xi }{)}^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }{)}^{\wedge })\\ \exp ({\mathbf{\xi }}^{\wedge })\exp (\Delta {\mathbf{\xi }}^{\wedge })\approx \exp (({\mathcal{J}}_r(\mathbf{\xi }{)}^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }{)}^{\wedge })\end{array}.\text{\hspace{1em}(2.14)}$$这里${\mathcal{J}}_l$形式较为复杂，是一个$6\times 6$的矩阵。
同理，我们在李代数上进行加法，让$\mathbf{\xi }$加上$\Delta \mathbf{\xi }$，那么可以近似为利群上带左右雅可比的乘法：$$\exp ((\mathbf{\xi }+\Delta \mathbf{\xi }{)}^{\wedge })=\exp (({\mathbf{J}}_l(\mathbf{\xi })\Delta \mathbf{\xi }{)}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })=\exp ({\mathbf{\xi }}^{\wedge })\exp (({\mathbf{J}}_r(\mathbf{\xi })\Delta \mathbf{\xi }{)}^{\wedge }).\text{\hspace{1em}(2.15)}$$接下来我们将讨论并对比李代数求导和李导数扰动模型求导[左乘]两种方法的异同。

3 李代数求导(SO(3)为例)

以$SO(3)$上的情况为例，我们假设对一个空间点$\mathbf{p}$进行了旋转，得到了$\mathbf{Rp}$，接下来我们将计算旋转之后的坐标相对于旋转的导数，我们非正规的记为：$$\frac{\partial (\mathbf{Rp})}{\partial \mathbf{R}}.\text{\hspace{1em}(3.1)}$$这里只是记作一个记号，并不是按照矩阵微分来定义的导数。因为$SO(3)$没有加法，因此不能按照导数的定义进行计算。现设$\mathbf{R}$对应的李代数为$\mathbf{\varphi }$，我们从而计算：$$\frac{\partial (\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p})}{\partial \mathbf{\varphi }}.\text{\hspace{1em}(3.2)}$$

这里严格来说，在矩阵微分中，智能求行向量关于列向量的导数，所得的结果为一个矩阵。SLAM十四讲中写成了列向量对列向量的导数，这里可以先认为对分子进行转置，在对最后结果进行转置。这样可以让计算过程和计算结果变得简洁，因为不这样做的话，每一行的分子都需要加上转置符号。在这样的意义下，我们可以认为$\mathrm{d}(\mathbf{A}\mathrm{x})/\mathrm{d}\mathrm{x}=\mathbf{A}$。

按照导数定义，我们有$$\begin{array}{rl}\frac{\partial (\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p})}{\partial \mathbf{\varphi }}& =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\exp ((\mathbf{\varphi }+\delta \mathbf{\varphi }{)}^{\wedge })\mathbf{p}-\exp ({\mathbf{\varphi }}^{\wedge }\mathbf{p})}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\exp (({\mathbf{J}}_l\delta \mathbf{\varphi }{)}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\varphi }}^{\wedge }\mathbf{p})}{\delta \mathbf{\varphi }}这里用了式(2.13)的结果\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{(\mathbf{I}+({\mathbf{J}}_l\delta \mathbf{\varphi }{)}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\varphi }}^{\wedge }\mathbf{p})}{\delta \mathbf{\varphi }}\exp (({\mathbf{J}}_l\delta \mathbf{\varphi }{)}^{\wedge })泰勒展开，舍去高阶项\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{({\mathbf{J}}_l\delta \mathbf{\varphi }{)}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{-(\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}){\mathbf{J}}_l\delta \mathbf{\varphi }}{\delta \mathbf{\varphi }}上一步到这步用了{\mathbf{a}}^{\wedge }\mathbf{b}\cdot \mathbf{c}=\mathbf{a}\times \mathbf{b}\cdot \mathbf{c}=-\mathbf{b}\cdot \mathbf{c}\times \mathbf{a}\\ & =-(\mathbf{Rp}{)}^{\wedge }{\mathbf{J}}_l.\end{array}\text{\hspace{1em}(3.3)}$$根据上式可以看出，结果中仍然含有形式比较复杂的${\mathbf{J}}_l$，我们不希望去计算它，因此我们下面介绍的扰动模型求导即为更简单的导数计算方式。

4 扰动模型求导(左乘)

4.1 SO(3)上的扰动模型求导

这里介绍的求导方式是对旋转矩阵$\mathbf{R}$进行一次扰动$\Delta \mathbf{R}$，然后观察结果想对于扰动的变化率。这个扰动可以左乘也可以右乘，不过最后结果会有微小的差异，这里我们以左扰动(左乘)为例。假设左扰动$\Delta \mathbf{R}$的李代数为$\mathbf{\phi }$。对$\mathbf{\phi }$求导，得：$$\begin{array}{rl}\frac{\partial (\mathbf{Rp})}{\partial \mathbf{\phi }}& =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp ({\mathbf{\phi }}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{(\mathbf{I}+{\mathbf{\phi }}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{{\mathbf{\phi }}^{\wedge }\mathbf{Rp}}{\mathbf{\phi }}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{-(\mathbf{Rp}{)}^{\wedge }\mathbf{\phi }}{\mathbf{\phi }}=-(\mathbf{Rp}{)}^{\wedge }\end{array}\text{\hspace{1em}(3.4)}$$明显可以看出，相对于李代数求导，对扰动模型求导可以省去一个雅可比${\mathbf{J}}_l$的计算，这就是扰动模型的实用性。

4.2 SE(3)上的扰动模型求导

由于李代数上求导的复杂性，这里直接给出$SE(3)$的扰动模型，我们假设某空间点$\mathbf{p}$经过一次变换$\mathbf{T}$(李代数为$\mathbf{\xi }$)，得到$\mathbf{Tp}$(这里$\mathbf{p}$为齐次坐标)。现在，让$\mathbf{T}$左乘一个扰动$\Delta \mathbf{T}=\exp (\delta {\mathbf{\xi }}^{\wedge })$，这里我们设扰动项的李代数为$\delta \mathbf{\xi }=[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^T$，根据导数定义有：$$\begin{array}{rl}\frac{\partial (\mathbf{Tp})}{\partial \delta \mathbf{\xi }}& =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp (\delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\delta {\mathbf{\xi }}^{\wedge }\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\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}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\mathbf{p}}{\delta \mathbf{\xi }}这里\exp ({\mathbf{\xi }}^{\wedge })=\mathbf{T}\\ & =\underset{\delta \mathbf{\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{Rp}+\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{Rp}+\mathbf{t})+\delta \mathbf{\rho }\\ 0^T\end{array}\right]}{[\delta \mathbf{\xi },\delta \mathbf{\rho }{]}^T}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}-(\mathbf{Rp}+\mathbf{t}{)}^{\wedge }\delta \mathbf{\varphi }+\delta \mathbf{\rho }\\ 0^T\end{array}\right]}{[\delta \mathbf{\xi },\delta \mathbf{\rho }{]}^T}上一步到这步用了{\mathbf{a}}^{\wedge }\mathbf{b}\cdot \mathbf{c}=\mathbf{a}\times \mathbf{b}\cdot \mathbf{c}=-\mathbf{b}\cdot \mathbf{c}\times \mathbf{a}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{Rp}+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\left[\begin{array}{c}\delta \mathbf{\rho }\\ \delta \mathbf{\varphi }\end{array}\right]}{[\delta \mathbf{\xi },\delta \mathbf{\rho }{]}^T}=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{Rp}+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\left[\begin{array}{c}\delta \mathbf{\rho }\\ \delta \mathbf{\varphi }\end{array}\right]}{\left[\begin{array}{c}\delta \mathbf{\rho }\\ \delta \mathbf{\varphi }\end{array}\right]}\\ & =\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{Rp}+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\stackrel{\mathrm{def}}{=}(\mathbf{Tp}{)}^{\odot }\end{array}\text{\hspace{1em}(3.5)}$$在这里，我们把最后的结果定义成一个算符${}^{\odot }$，他把一个齐次坐标的空间点变换成一个$4\times 6$的矩阵。
附：
最后我们在这里简单解释一下上述定义矩阵求导的顺序，假设$\mathbf{a},\mathbf{b},\mathbf{x},\mathbf{y}$都是列向量，在我们上述符号的写法下，有如下规则：$$\frac{d\left[\begin{array}{c}\mathbf{a}\\ \mathbf{b}\end{array}\right]}{d\left[\begin{array}{c}\mathbf{x}\\ \mathbf{y}\end{array}\right]}={\left(\begin{array}{c}\frac{d\left[\begin{array}{cc}\mathbf{a}& \mathbf{b}\end{array}\right]}{d\left[\begin{array}{c}\mathbf{x}\\ \mathbf{y}\end{array}\right]}\end{array}\right)}^T={\left[\begin{array}{cc}\frac{d\mathbf{a}}{d\mathbf{x}}& \frac{d\mathbf{b}}{d\mathbf{x}}\\ \frac{d\mathbf{a}}{d\mathbf{y}}& \frac{d\mathbf{b}}{d\mathbf{y}}\end{array}\right]}^T=\left[\begin{array}{cc}\frac{d\mathbf{a}}{d\mathbf{x}}& \frac{d\mathbf{a}}{d\mathbf{y}}\\ \frac{d\mathbf{b}}{d\mathbf{x}}& \frac{d\mathbf{b}}{d\mathbf{y}}\end{array}\right]$$

---

创作不易，希望大家支持，多多点赞收藏！！！！非常感谢！！！！

参考资料：
维基百科-BCH公式
视觉SLAM十四讲：从理论到实践(第2版)(ISBN：9787121369421)
旋量代数与李群、李代数 戴建生(ISBN：9787040318456)