《视觉SLAM14讲》学习小结——供小白快速入门ch4

Zealot[胜利][憨笑][疑问]

已于 2024-08-02 13:47:56 修改

阅读量559

点赞数 4

分类专栏：《视觉SLAM14讲》学习小结——供小白快速入门：理论部分文章标签：学习

于 2024-07-28 01:41:04 首次发布

本文链接：https://blog.youkuaiyun.com/weixin_44469383/article/details/140727936

版权

《视觉SLAM14讲》学习小结——供小白快速入门：理论部分专栏收录该内容

13 篇文章

订阅专栏

李群和李代数

还记得我们在ch2章中指出，SLAM的本质是一个优化问题。视觉SLAM就是根据摄像机的观测结果，通过优化的方法来确定摄像机位姿和特征点位置的一个过程。在ch3的介绍中我们知道，摄像机的姿态可用变换矩阵 $\mathbf{R}$ 来表示，也可以用旋转矢量 $\vec{q}$ 来表示。 $\mathbf{R}$ 是一个 $3\times3$ 的矩阵含有9个未知数，而 $\vec{q}$ 只含有3个未知数。因此 $\mathbf{R}$ 的表示法存在信息冗余，其中的元素应当满足某些约束，如正交阵的各种约束。这种约束使得SLAM中的优化问题是带约束的，给实际执行带来困难。为了将这种有约束优化问题转换成无约束优化问题，需要用到本章所讨论的李群和李代数等数学知识。

一、李群

所谓的群，就是定义在一个集合和一种运算上的代数结构。比如整数和加法就构成了一种群，去掉零的有理数和乘法也构成群。并不是任意一个集合加一种运算都能构成群，这种定义在集合上的运算必须满足如下“封结幺逆”条件，读者可用上述两个群代入协助理解

（1）封闭性：两个元素的运算结果仍属于这个集合。

（2）结合律：以加法为例 $a+b+c=a+(b+c)$ ，因此满足结合律。矩阵的叉乘就不满足结合律，即 $A\times B\times C\neq A\times (B\times C)$ 。

（3）幺元：存在一个元素 $a_0$ ，这个元素与集合中任意元素 $x$ 的运算结果仍等于这个 $x$ 。整数加法群中的 $0$ 和有理数乘法群中的 $1$ 都是幺元。

（4）逆：对于任意一个元素，在集合中一定可以找到另外一个元素与此元素的运算结果等于幺元。对于整数加法群，逆就是求相反数；对于去零有理数乘法群，逆就是求倒数。

李群是指具有连续性质的群，例如前面所说的整数加法群就不是连续群。本书主要用到的李群是三维旋转变换群 $SO(3)$ 和三维欧氏变换群 $SE(3)$ 。 $SO(3)$ 群中的元素为坐标变换矩阵 $\mathbf{R}$ ，运算为矩阵乘法。 $SE(3)$ 中的元素为坐标变换矩阵 $\mathbf{T}$ ，运算也是矩阵乘法。关于 $\mathbf{R},\mathbf{T}$ 的概念可参考上一篇博客ch3。两个正交阵之和并不一定是正交阵，因此无法在 $SO(3)$ 或 $SE(3)$ 的集合上定义加法运算。如此一来更无法定义其中元素的微分或导数，这是因为微分的本质是差分的极限。SLAM优化中需要不断改变变换矩阵的值以寻找目标函数的最优值，因此需要用到变换矩阵的微分或导数运算。为了解决这个问题，引入李代数的概念。

二、李代数

李代数由一个集合 $V$ ，一个数域 $F$ 和一个二元运算 $[\cdot ,\cdot ]$ 组成，并满足如下条件。三维向量 $R^3$ ，实数和叉乘运算就构成了一个李代数，读者可代入以下各条验证。

（1）封闭性，集合中任意两个元素经二元运算后的结果仍在集合中。

（2）双线性，两个集合的线性组合与第三个集合作二元运算，等于这两个集合分别与第三个集合作二元运算的线性组合。即 $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in V, a,b\in F$ 有

$[a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a[\mathbf{X},\mathbf{Z}]+b[\mathbf{Y},\mathbf{Z}],~ [\mathbf{Z},a\mathbf{X}+b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}]+b[\mathbf{Z},\mathbf{Y}]$

（3）自反性，集合中元素与自身的二元运算结果为零。

（4）雅可比等价： $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in V$

$[\mathbf{X},[\mathbf{Y},\mathbf{Z}]]+[\mathbf{Z},[\mathbf{X},\mathbf{Y}]]+[\mathbf{Y},[\mathbf{Z},\mathbf{X}]]=0$

其中的二元运算 $[\cdot ,\cdot ]$ 又称为李括号。

常见的李代数有 $so(3)$ 和 $se(3)$ 。为了解释这两种李代数，需要先定义一种李括号。前面已经介绍过叉乘矩阵，即任意一个列阵 $\phi=[\phi_1,\phi_2,\phi_3]^\mathrm{T}$ 按如下方式排列得到

$\Phi=\phi^\wedge=\begin{bmatrix} 0 &-\phi_3 &\phi_2 \\ \phi_3&0 &-\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{bmatrix}$ (1)

根据上式可知，已知一个叉乘矩阵也可反求对应的列阵，记为 $\phi=\Phi^\vee$ 。在此定义下，两个列阵 $\phi_1,\phi_2$ 的李括号为

$[\phi_1,\phi_2]=(\Phi_1\Phi_2-\Phi_2\Phi_1)^\vee$ (2)

至此，我们可以定义 $so(3)$ 。它是一种李代数，其集合元素是三维坐标列阵 $\phi$ ，它的数域是实数，它的二元运算或李括号为式（2）。

类似地，李代数 $se(3)$ 也是一种李代数，其集合元素是六维坐标列阵 $\xi=[\rho,\phi]^\mathrm{T}$ ，其中三维坐标列阵 $\rho$ 表示平移。 $\phi$ 就是 $so(3)$ 中的元素。它的数域也是实数，它的李括号为

$[\xi_1,\xi_2]=(\xi_1^\wedge\xi_2^\wedge-\xi_2^\wedge\xi_1^\wedge)^\vee$ (3)

其中

$\xi^\wedge=\begin{bmatrix} \phi^\wedge &\rho \\ \mathbf{0}^\mathrm{T}& 0 \end{bmatrix}$ (4)

需要补充说明的是，向量和矢量是同一个概念，向量或矢量与坐标列阵或矩阵是完全不同的概念。为了区分起见，本博客中使用了不同的符号予以区分，矢量一般加上箭头。高翔博士著作中常用向量来称呼坐标列阵，这可能是某种习惯，大家要注意。数组是计算机行业的用语，可用于在计算机中表征矩阵。

三、李群与李代数之间的映射关系

1.映射方法

我们知道李群 $SO(3)$ 中的元素为旋转矩阵或变换矩阵 $\mathbf{R}$ ，李代数 $so(3)$ 中的元素为 $\phi$ ，这二者之间有什么关系呢？研究表明，这种关系为

$\mathbf{R}=\exp{(\phi^\wedge)}$ (5)

即 $so(3)$ 中的元素坐标列阵 $\phi$ ，转换为叉乘矩阵后再求指数运算就可以得到 $SO(3)$ 中的元素旋转矩阵 $\mathbf{R}$ ，我们简称这种关系为指数映射。反过来，在已知 $\mathbf{R}$ 的情况下可通过对数映射求 $\phi$ ，即

$\phi=\ln(\mathbf{R})^\vee$ (6)

现在想想我们为什么要引入李代数 $so(3)$ 呢？我们前面讲到，李群 $SO(3)$ 中的元素不满足加法运算但满足乘法运算。对数可以将实数乘法运算转换成加法运算。若此规律对矩阵也适用，则李群 $SO(3)$ 通过式（6）映射到李代数 $so(3)$ 上后，其元素 $\phi$ 就会满足加法运算，从而为导数的定义提供可能。

类似地，李群 $SE(3)$ 中的元素为旋转矩阵或变换矩阵 $\mathbf{T}$ ，李代数 $se(3)$ 中的元素为 $\xi$ ，它们之间的映射关系为

$\mathbf{T}=\exp{(\xi^\wedge)}$ (7)

其逆映射为

$\xi=\ln(\mathbf{T})^\vee$ (8)

2.计算方法

我们虽然明确了李群和李代数之间的关系，然而还不清楚如何具体计算式（5-8）中的 $\exp{},\ln{}$ 函数。这并不是实数的指数和对数函数，而是矩阵的指数和对数函数，因此需要进一步明确。对于式（5），其具体计算方法就是前面介绍的罗德里格斯公式。令 $\phi=\theta\mathbf{n}$ ，则

$\mathbf{R}=\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{n}\mathbf{n}^\mathrm{T}+\sin{\theta}\mathbf{n}^{\wedge}$ (9)

式（6）的具体计算方法为

$\theta=\arccos\frac{tr(\mathbf{R})-1}{2},~\mathbf{R}\mathbf{n}=\mathbf{n}$ (10)

对于式（7），计算时先根据 $\xi=[\rho,\phi]^\mathrm{T}$ 分离得到 $\rho$ 和 $\phi=\theta\mathbf{n}$ ，然后式（9）计算 $\mathbf{R}$ ，按下式计算中间变量 $\mathbf{J}$

$\mathbf{J} = \frac{\sin{\theta}}{\theta}\mathbf{I}+\left(1- \frac{\sin{\theta}}{\theta}\right)\mathbf{n}\mathbf{n}^\mathrm{T}+\frac{1-\cos{\theta}}{\theta}\mathbf{n}^\wedge$ (11)

最后按下式计算 $\mathbf{T}=\exp{(\xi^\wedge)}$ 的最终结果

$\mathbf{T}=\begin{bmatrix} \mathbf{R} &\mathbf{J}\rho \\ \mathbf{0}^\mathbf{T}&1 \end{bmatrix}$ (12)

在已知 $\mathbf{T}$ 的情况下计算 $\mathbf{\xi}$ 的过程也不难。先从 $\mathbf{T}$ 左上角取出 $\mathbf{R}$ ，然后根据式（10）计算得到 $\theta,\mathbf{n}$ 的值。接着计算 $\phi=\theta\mathbf{n}$ 并根据式（11）求得 $\mathbf{J}$ 。最后从 $\mathbf{T}$ 左上角取出 $\mathbf{t}=\mathbf{J}\rho$ ，因为现在 $\mathbf{t},\mathbf{J}$ 均为已知，所以这是一个关于 $\rho$ 的线性方程组，不难求得。在确定 $\phi$ 和 $\rho$ 的情况下，按 $\mathbf{\xi}=[\rho,\phi]^\mathrm{T}$ 排列即可。

四、坐标对变换矩阵的求导方法与比较

这里第1小节先给出讨论位矢对变换矩阵的求导的目的和方法。第2小节分析李代数求导和扰动模型法的区别。第2节内容比较难，可暂时跳过。

1.坐标对变换矩阵的求导方法

在前篇ch2中已提到SLAM实质上是一个优化问题并给出了优化模型。对于视觉SLAM而言，由于作用在相机上的外力未知，一般不采用运动方程，这样优化模型就只剩下量测方程。根据博文ch3中二、4节的分析，在考虑量测噪声的情况下量测方程可写为

$\mathbf{z}=\mathbf{T}\mathbf{P}^w+\mathbf{w}$ (13)

其中 $\mathbf{w}$ 为量测噪声， $\mathbf{z}$ 为观测结果，即 $\mathbf{a}$ 的齐次坐标。 $\mathbf{\hat{P}}^w$ 即 $\mathbf{a}^\prime$ 为世界坐标系中的齐次坐标，这里按计算机视觉的惯例不与世界坐标系中的坐标 $\mathbf{P}^w$ 区分。 $\mathbf{T}=\mathbf{T}_{21}$ 为坐标变换矩阵。为了求解最优的相机位姿 $\mathbf{T}$ ，令 $\mathbf{e}=\mathbf{z}-\mathbf{T}\mathbf{P}^w$ 。为了简化起见，这里先假设 $\mathbf{a}^\prime$ ，即路标的世界坐标是已知的。对于每一个路标点或观测，都可得到一个方程共N个。优化的目的是通过调整相机的位姿或 $\mathbf{T}$ 的值，使得正体误差最小，即

$\min\limits_{\mathbf{T}} J(\mathbf{T})=\sum\limits_{i=1}^N|\mathbf{z}_i-\mathbf{T}\mathbf{P}^w_i|_2^2$ (13)

取极值的方法是先求驻点即一阶导数，那么应该如何求 $\partial(\mathbf{T}\mathbf{P}^w)/\partial \mathbf{T}$ 呢？我们已经知道 $SE(3)$ 上没有定义加法，因此 $\partial \mathbf{T}$ 没有定义，即无法计算 $\partial(\mathbf{T}\mathbf{P}^w)/\partial \mathbf{T}$ 。由于 $\mathbf{T}$ 的信息浓缩在 $\xi$ 中，因此可将 $J(\mathbf{T})$ 理解为 $J(\xi)$ ，进而得到 $\partial(\mathbf{T}\mathbf{P}^w)/\partial \mathbf{T}$ 的具体算法。这种具体算法有两种，有兴趣的读者可看下一小节的进一步分析。常用的那种称为扰动模型法，其结果为

$\frac{\partial(\mathbf{T}\mathbf{P}^w)}{\partial\delta\xi}=\begin{bmatrix} \mathbf{I} &-(\mathbf{R}\mathbf{P}^w+\mathbf{t})^\wedge \\ \mathbf{0}^\mathrm{T}& 1 \end{bmatrix}\doteq (\mathbf{T}\mathbf{P}^w)^{\odot }$ （14）

对于只有旋转的情况，扰动模型法的结果为

$\frac{\partial(\mathbf{T}\mathbf{P}^w)}{\partial\delta\phi}= -(\mathbf{R}\mathbf{P}^w)^\wedge$ （15）

2.*不同求导方法的比较与讨论

根据导数的定义， $\partial(\mathbf{T}\mathbf{P}^w)/\partial \mathbf{T}$ 表示 $\mathbf{T}$ 发生微小变化后 $\mathbf{T}\mathbf{P}^w$ 的变化量。那么我们如何让 $\mathbf{T}$ 发生微小变化呢？显然不能让 $\mathbf{T}$ 中的元素任意发生微小变化，这是因为 $\mathbf{T}$ 不满足加法法则， $\mathbf{T}$ 中元素任意发生变化后不能保证它仍然是变换矩阵。第一种办法是让 $\xi$ 发生微小变化 $\mathrm{d}\xi$ ，这样 $\mathbf{T}$ 就会变成 $\boldsymbol{\xi}+\mathrm{d}\boldsymbol{\xi}$ 所对应的值 $\mathbf{T}_{new}$ ，进而可计算 $\partial(\mathbf{T}\mathbf{P}^w)/\partial \mathbf{T}\doteq [\mathbf{T}_{new}\mathbf{P}^w-\mathbf{T}\mathbf{P}^w]/\mathrm{d}\boldsymbol{\xi}$ 。其结果记为 $\partial(\mathbf{T}\mathbf{P}^w)/\partial\boldsymbol{\xi}$ ，称为李导数求导方法。这种方法所得结果比较复杂，不利于实际应用。第二种方法是先假设 $\mathbf{T}$ 的变化量可用一个变换矩阵 $\delta\mathbf{T}$ 左乘来表征，即变化后的变化矩阵为 $\delta\mathbf{T}*\mathbf{T}$ 。由于 $\mathbf{T}$ 满足乘法法则，因此 $\delta\mathbf{T}*\mathbf{T}$ 仍然是一个变换矩阵，可求得其对应的 $\xi_{new}$ 。计算 $[(\delta\mathbf{T}*\mathbf{T})\mathbf{P}^w-\mathbf{T}\mathbf{P}^w]/(\boldsymbol{\xi}_{new}-\boldsymbol{\xi})$ 并求极限可得 $\partial(\mathbf{T}\mathbf{P}^w)/\partial \mathbf{T}$ 的第二种计算方法。其结果记为 $\partial(\mathbf{T}\mathbf{p})/\partial\delta\boldsymbol{\xi}$ ，称为扰动模型法。

为什么两种导数计算方法的结果会不同呢？其本质是两种求导方法中的自变量不同。李导数求导时的自变量是 $\xi$ ，而扰动模型求导数时的自变量是 $\mathbf{T}$ 。举一个简单的例子，用x表示 $\xi$ ，用y表示 $\mathbf{T}$ ，设二者关系为 $y=2x^2$ 。用 $z=f(y)=5+y^3$ 表示 $\mathbf{T}\mathbf{P}^w$ 。计算李导数相当于求 $\mathrm{d} z/\mathrm{d} x$ ，其结果为

$\frac{\mathrm{d}z}{\mathrm{d}x}=\frac{\mathrm{d}z}{\mathrm{d}y}\frac{\mathrm{d}y}{\mathrm{d}x}=3y^2*4x=12xy^2$ （16）

用扰动模型计算导数时相当于计算 $\mathrm{d} z/\mathrm{d} y$ ，其结果为

$\frac{\mathrm{d}z}{\mathrm{d}y}=3y^2$ （17）

两个结果显然不同，二者相差 $\mathrm{d} y/\mathrm{d} x=4x$ 。

这两种不同的求导方法会对优化结果产生影响吗？显然，我们如果以 $\xi$ 或x为自变量时，当然要选择李导数的求导方法。如果选择 $\mathbf{T}$ 或y为自变量时，就要选择扰动模型法来计算导数。两种导数的计算结果仅相差 $\partial \mathbf{T}/\partial \boldsymbol{\xi}$ 或 $\mathrm{d} y/\mathrm{d} x=4x$ 。在优化时，若 $\boldsymbol{\xi}$ 或 $\mathbf{T}$ 仅发生微小的变化（在每一步的迭代中事实也是如此），则这 $\partial \mathbf{T}/\partial \boldsymbol{\xi}$ 或 $\mathrm{d} y/\mathrm{d} x=4x$ 可视为固定，因此采用不同的求导方法来优化一般不会对结果产生影响。

上面论述过程中一方面强调不能之间对 $\mathbf{T}$ 求导，一方面又说扰动模型其实就是对 $\mathbf{T}$ 求导，这是否存在矛盾？前者强调的是求导方法，后者强调的是求导对象。事实上，两种求导方法中自变量都是 $\xi$ ，李导数求导方法中应变量的变化是根据 $\xi$ 的变化计算，而扰动模型法中应变量的变化是根据 $\mathbf{T}$ 的变化来计算。

五、总结

视觉SLAM的本质是一个静态优化问题，优化变量为变换矩阵 $\mathbf{T}$ 。优化过程中需要将目标函数对优化变量求导。由于 $\mathbf{T}$ 不满足加法法则，无法直接定义求导方法。通过引入李导数，将欧式群 $SE(3)$ 映射到李代数 $se(3)$ 上，从而使得导数计算变得可能。求导方法有两种，在实际应用中，我们常使用扰动模型法，其结果如式（14）和（15）所示。