一、解决的问题
如前所述,相机的位姿由6个自由度来表示,转换成数学描述就是由旋转矩阵和变换矩阵 R , t {R,t} R,t描述,在SLAM中位姿是未知的,我们需要解决形如“什么样的相机位姿最符合当前观测数据”,即一种典型的方式是把它构建成一个优化问题,求解最优的 R , t {R,t} R,t,使得误差最小化,即对位姿进行估计和优化。这里之所以引入李群和李代数是因为旋转矩阵自身是带有约束的(正交且行列式为1),它们作为优化变量时会引入额外的约束,使优化变得困难,通过李群和李代数间的转换关系,我们可以把位姿估计变成无约束优化问题。
二、李群和李代数
简单来说我们希望用李群和李代数的方法来优化位姿问题。
2.1 李群和李代数的基础
如前所述我们用旋转矩阵和变换矩阵来描述位姿,三维旋转矩阵构成了特殊正交群
S
O
(
3
)
{SO(3)}
SO(3),而变换矩阵构成了特殊欧式群
S
E
3
{SE3}
SE3,如下:
但他们对加法不封闭,对乘法封闭即:
- 群
群是一种集合加上一种运算的代数结构,满足以下条件:
而李群是指具有连续光滑性质的群,而 S O ( n ) {SO(n)} SO(n)和 S E ( n ) {SE(n)} SE(n)在实数空间上是连续的,我们能够直观地想象一个刚体能够连续地在空间中运动,所以它们都是李群。 - 李代数
每个李群都有与之对应的李代数,李代数描述了李群的局部性质,李代数定义如下:
s o ( 3 ) {so(3)} so(3)和 s e ( 3 ) {se(3)} se(3)上的李代数为:
综上李代数的定义与相互转换关系如下:
- 李代数求导与扰动模型
迄今为止我们采用李代数的主要原因是为了优化,通常求导是优化时的必要手段,我们来看下李代数的求导。
S O ( 3 ) {SO(3)} SO(3)上的李代数求导,通常李代数解决求导问题的思路分为两种:- 用李代数表示姿态,然后根据李代数加法对李代数求导。
- 对李群左乘或右乘微小扰动,然后对该扰动求导,称为左扰动和右扰动
-
李代数求导
-
扰动模型(左乘)
-
S
E
(
3
)
{SE(3)}
SE(3)上的李代数求导
三、评估轨迹误差
在实际工程中,我们经常需要评估算法的估计轨迹与真实轨迹的差异来评价算法的精度。真实轨迹往往通过某些更高精度的系统获得,而估计轨迹是由待评价的算法计算得到的。常见的误差指标有绝对轨迹误差(Absolute Trajectory Error,ATE),形如:
这实际上是每个位姿李代数的均方根误差。这种误差可以刻画两条轨迹的旋转和平移误差。
总结
本讲引入了李群 S O ( 3 ) {SO(3)} SO(3)和 S E ( 3 ) {SE(3)} SE(3),以及它们对应的李代数 s o ( 3 ) so(3) so(3)和 s e ( 3 ) se(3) se(3)。我们介绍了位姿在他们上面的表达和转换,然后通过BCH的线性近似,就可以对位姿进行燃动并求导。这给之后讲解位姿的优化打下了理论基础,因为们需要经常对某一个位姿的估计值进行调整,使它对应的误差减小。只有在弄清楚如何对位姿进行调整和更新之后,我们才能继续下一步的内容。