【视觉SLAM十四讲学习笔记】第四讲——李代数求导与扰动模型

发布时间:2023年12月18日

专栏系列文章如下:
【视觉SLAM十四讲学习笔记】第一讲——SLAM介绍
【视觉SLAM十四讲学习笔记】第二讲——初识SLAM
【视觉SLAM十四讲学习笔记】第三讲——旋转矩阵
【视觉SLAM十四讲学习笔记】第三讲——旋转向量和欧拉角
【视觉SLAM十四讲学习笔记】第三讲——四元数
【视觉SLAM十四讲学习笔记】第三讲——Eigen库
【视觉SLAM十四讲学习笔记】第四讲——李群与李代数基础
【视觉SLAM十四讲学习笔记】第四讲——指数映射

BCH公式与近似形式

使用李代数的一大动机是进行优化,而在优化过程中导数是非常重要的信息。虽然我们已经清楚了SO(3)和SE(3)上的李群与李代数关系,但是当在SO(3)中完成两个矩阵乘法时,李代数中so(3)上发生了什么改变呢?反过来说,当so(3)上做两个李代数的加法时,SO(3)上是否对应着两个矩阵的乘积?

如果成立,相当于:
在这里插入图片描述
如果?_1,?_2为标量,那么显然该式成立;但此处我们计算的是矩阵的指数函数,而非标量的指数。换言之,我们在研究下式是否成立:
在这里插入图片描述
很遗憾,该式在矩阵时并不成立。两个李代数指数映射乘积的完整形式,由BCH公式给出。由于其完整形式较复杂,我们只给出其展开式的前几项,其中[ ]为李括号:
在这里插入图片描述
上面的BCH公式告诉我们,当处理两个矩阵指数之积时,它们会产生一些由李括号组成的余项。特别地,考虑SO(3)上的李代数ln(exp(?_1^)exp(?_2 ^))∨,当?_1或?_2为小量时,小量二次以上的项都可以被忽略。此时,BCH拥有线性近似表达:
在这里插入图片描述
以第一个近似为例。该式告诉我们,当对一个旋转矩阵R_2(李代数为?_2)左乘一个微小旋转矩阵R_1(李代数为?_1)时,可以近似地看作,在原有的李代数?_2上加上了一项J_l(?_2)-1?_1。同理,第二个近似描述了右乘一个微小位移的情况。于是,李代数在BCH近似下,分成了左乘近似和右乘近似两种。而右乘雅可比仅需要对自变量取负号即可:
在这里插入图片描述
这样,我们就可以谈论李群乘法和李代数加法的关系了。为了方便理解,我们重新叙述BCH近似的意义。假定对于某个旋转R,对应的李代数为?。我们给他左乘一个微小旋转,记作ΔR,对应的李代数为Δ?。那么,在李群上,得到的结果就是ΔR·R,而在李代数上,根据BCH近似,为J_l-1(?) Δ?+?。合并起来,可以简单地写成:
在这里插入图片描述
反之,如果我们在李代数上进行加法,让一个?加上Δ?,那么可以近似为李群上带左右雅可比的乘法:
在这里插入图片描述
这就为之后李代数上做微积分提供了理论基础。同样地,对于SE(3),也有类似的BCH近似:
在这里插入图片描述

SO(3)上的李代数求导

在SLAM中,要估计一个相机的位置和姿态,该位姿是由SO(3)上的旋转矩阵或SE(3)上的变换矩阵描述的。设某个时刻机器人的位姿为T,它观察到了一个世界坐标位于p的点,产生了一个观测数据z。由坐标变换关系知:
在这里插入图片描述
其中w为随机噪声。由于它的存在,z 往往不可能精确地满足z=Tp的关系。所以通常会计算理想的观测与实际数据的误差:
在这里插入图片描述假设一共有N个这样的路标点和观测,于是就有N个上式。那么,对机器人的位姿估计,相当于是寻找一个最优的T,使得整体误差最小化:
在这里插入图片描述
求解此问题,需要计算目标函数J关于变换矩阵T的导数。重点是构建与位姿有关的函数,讨论该函数关于位姿的导数,以调整当前的估计值。然而SO(3),SE(3)上并没有良好定义的加法,它们是群。如果把T当成一个普通矩阵来处理优化,那就必须对它加以约束(旋转矩阵的约束是行列式值唯一,计算复杂)。而从李代数角度来说,由于李代数由向量组成,具有良好的加法运算。

因此,使用李代数解决求导问题的思路分为两种:

  1. 用李代数表示姿态,然后根据李代数加法来对李代数求导

  2. 对李群左乘右乘微小扰动,然后对该扰动求导,称为左扰动和右扰动模型。

第一种方式对应到李代数的求导模型,而第二种则对应到扰动模型。

李代数求导

首先,考虑SO(3)上的情况。假设对一个空间点p进行了旋转,得到了Rp。计算旋转之后点的坐标相对于旋转的导数,我们非正式地记为 :
在这里插入图片描述
由于SO(3)没有加法,所以该导数无法按照导数的定义进行计算。设R对应的李代数为?,转而计算:
在这里插入图片描述
按照导数的定义,推导出了旋转后的点相对于李代数的导数:
在这里插入图片描述
不过,由于这里仍然含有形式比较复杂的雅可比式,我们不太希望计算它。而下面的扰动模型则提供了更简单的导数计算方式。

扰动模型(左乘)

另一种求导方式是对R进行一次扰动ΔR,看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边,最后结果会有一点微小的差异,我们以左扰动为例。设左扰动ΔR对应的李代数为φ。然后对φ求导,即:
在这里插入图片描述
相比于直接对李代数求导,省去了一个雅可比矩阵的计算。这使得扰动模型更为实用,在位姿估计当中具有重要的意义。

SE(3)上的李代数求导

最后,我们给出SE(3)上的扰动模型,而直接李代数上的求导就不再介绍了。假设某空间点p经过一次变换T(对应李代数为ξ),得到TP

现在,给T左乘一个扰动?T = exp(δξ∧),设扰动项的李代数为 δξ = [δρ?]T,那么:
在这里插入图片描述
我们把最后的结果定义成一个算符,它把一个齐次坐标的空间点变换成一个4×6的矩阵。

文章来源:https://blog.csdn.net/PuddleRubbish/article/details/134989297
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。