@ 《视觉SLAM十四讲》知识点与习题
《视觉SLAM十四讲》第四讲知识点整理+习题
正在学习SLAM相关知识,将一些关键点及时记录下来。
知识点整理
本讲主要解决**什么样的相机位姿最符合当前观测数据**问题。一种典型的方法是把它构建成一个优化问题,求解最优的R,t,使得误差最小化。通过李群-李代数间的转换关系,可以将位姿估计变成无约束的优化问题,简化求解方式
- 群:一种集合加上一种运算的代数结构。满足以下条件“封结幺逆”
- 特殊正交群SO(3): 三维空间的旋转矩阵,是一个正交阵,且行列式为1,即旋转矩阵群
- 特殊欧氏群SE(3): 包含三维空间旋转矩阵R,平移向量t,0^T 和1的四维空间变换矩阵。即3维欧式变换群
- 李群: 具有连续(光滑)性质的群。每个李群都有对应的李代数
- 反对称矩阵: A = -A_T。对于一个向量,可以将其变成反对称矩阵;对于任意反对称矩阵,亦可以找到一个与之对应的向量
- Φ叫做:在SO(3)原点附近的正切空间。R(t) = exp(Φ^ t),这里Φ是指Φ0. 其中^是指将Φ0向量变成其对应的反对称矩阵的操作。所以,每对旋转矩阵求一次导数,只需要左侧乘以Φ(t0)即可
- 李代数: 描述了李群的局部性质。由一个集合,一个数域,一个二元运算组成。具有封闭性,双线性,自反性和雅可比等价。SO(3)对应的李代数是定义在3D空间上的向量。由于李代数和反对称矩阵关系很紧密,在不引起歧义的情况下,就说李代数的元素是三维向量或者三维矩阵。李代数so(3) 是一个三维向量。李代数se(3) 是一个六维向量
- 指数映射: 在李群和李代数中,表示一个矩阵的指数。任意矩阵的指数映射可以写成一个Taylor展开,但是只有在收敛的情况下才会有结果,其结果仍是一个矩阵
- 特殊正交群的李代数实际上就是所谓的旋转向量组成的向量,其指数映射即罗德里格斯公式。通过它们,可以把so(3)中任意一个向量对应到一个位于SO(3)中的旋转矩阵,反之也可以
指数映射: 。指数映射表示把so(3)中任意一个向量(即旋转向量)对应到了一个位于SO(3)中的旋转矩阵
对数映射:。对手映射表示将SO(3)中的元素对应到so(3)中去
使用李代数的一大动机就是为了进行优化,而在优化过程中导数是非常必要的信息 - 两个李代数指数映射乘积的完整形式: 由BCH公式给出。BCH公式线性近似表达:
以第一个近似为例。当对一个旋转矩阵R2(李代数为Φ2)左乘一个微小旋转矩阵R1(李代数为Φ1)时,可以近似的看做,在原有的李代数Φ2上,加上了一项。对于第二个近似同理。于是,李代数在BCH近似下,分成了左乘近似和右乘近似两种,在使用时需加注意,使用的是做成模型还是右乘模型
总结为:
- 李群和李代数之间的转换关系
- 使用李代数解决求导问题的思路: 应用中,通常会构建与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值。此时,就需要用到李代数求导。有两种解决思路
- 用李代数表示姿态,然后根据李代数加法来对李代数求导
- 对李群左乘或右乘微小扰动,然后对该扰动求导,称为左扰动和右扰动模型
- 李代数求导: 旋转后的点相对于李代数的导数。但是仍然含有形式比较复杂的Jl,所以还是很复杂
- 扰动模型(左乘):设左扰动△R对应的李代数为φ,对φ求导
取近似后的结果为
实践Sophus
- . Sophus基于Eigen进行开发,可以从旋转矩阵、旋转向量和四元数构造Sopuhs::SO(3)
- 使用对数映射可以获得对应的李代数,用Eigen::Vector3d表示
// 使用对数映射获得它的李代数
Eigen::Vector3d so3 = SO3_R.log();
cout<<"so3 = "<<so3.transpose()<<endl;
// hat 为向量到反对称矩阵
cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;
// 相对的,vee为反对称矩阵到向量
cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些
------------------------------------------------------------
// 增量扰动模型的更新。此时为左乘扰动模型
Eigen::Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R
输出为
- 查看Sophus源码中使用到的函数
1. SO(3)的hat函数源码,根据书上的公式直接赋值。这个就比较直观了,**********************************
2. SO(3)的vee函数源码,直接返回反对称矩阵中对应位置的值即可
3. SO(3)的exp函数。返回的是一个以四元数为输入的SO3变量。因此,对于四元数,根据公式[3.19],计算二分之一的旋转角度的cos和sin值即可。而旋转角度,根据下述代码,即为传入Omega的模长
4. SO(3)的log函数。相比较exp函数而言,log函数就复杂了,,,