一、基于BA的VIO融合
1.视觉SLAM中的BA问题
已知:状态量初始值(特征点的三维坐标,相机的位姿),系统测量值(特征点在不同图像上的图像坐标)
问题:如何估计状态量的最优值?
解决方式:构建误差函数,利用最小二程得到状态量的最优估计
q:旋转四元数 p:平移向量 f:特征点3D坐标 ,q p f本身是有下标的
:第i个相机系,
(·):投影函数,三维空间的点投影到相机平面上,如果是使用相机的像素坐标即为二维的,本次课程主要使用的是归一化平面(z=1)上的坐标
:
对
的观测,
:范数
2.VIO信息融合问题
IMU误差是通过IMU预积分构建的,在两幅图像之间会存在很多个IMU测量值,所以把这些进行积分得到一个约束
二、最小二乘问题的求解
1.最小二乘基础概念
定义:找到一个n维的变量,使得损失函数F(x)取局部最小值:
,其中fi是残差函数,比如测量值和预测值之间的差,且有m大于等于n。
局部最小值指对任意有
损失函数泰勒展开:假设损失函数F(x)可导并且平滑的,因此,二阶泰勒展开:
其中J和H分别为损失函数F对变量x的一阶导和二阶导矩阵。
雅各比矩阵J是一阶偏导数以一定方式排列成的矩阵,类似于多元函数的导数,相当于矩阵的一阶导数。
海塞矩阵H(Hessian Matrix)是多元函数的二阶偏导数构成的方阵,描述了函数的局部曲率,相当于矩阵的二阶导数。
损失函数泰勒展开的性质,忽略泰勒展开的高阶项,损失函数变成了二次函数
1)如果在点Xs处导数为0,则称这个点为稳定点。
2)在点Xs处对应的Hessian为H
3)如果是正定矩阵,即它的特征值都大于0,则在Xs处有F(x)为局部最小值
4)如果是负定矩阵,即它的特征值都小于0,则在Xs处有F(x)为局部最大值
5)如果是不定矩阵,即它的特征值有大于0也有小于0的,则在Xs处有F(x)为鞍点
鞍点:在泛函中,既不是极大值点也不是极小值点的临界点叫做鞍点
2.最小二乘法的求解
(1)迭代下降法求解:下降法
基本思想:找一个下降方向使损失函数随x的迭代逐渐减小,直到x收敛到x*:
第一,找到下降方向单位向量d,第二,确定下降步长α,假设α足够小,我们可以对损失函数F(x)进行一阶泰勒展开:
,只需寻找下降方向,满足Jd<0,通过line search方法找到下降的步长
line search,线搜索方法,是一种迭代的求得某个函数的最值得方法,对于每次迭代,线搜索会计算得到搜索的反向pk以及沿这个方向移动的步长αk.
(2)最速下降法(适用于迭代的开始阶段),从下降方向的条件可知:,
表示下降方向和梯度方向的夹角,当
,有
,即梯度的负方向为最速下降方向。缺点:最优值附近震荡,收敛慢
牛顿法(适用于最优值附近),在局部最优点x*附近,如果x+△x是最优解,则损失函数对△x的导数等于0,对公式取一阶导数有
,可以得到
,缺点为二阶导矩阵计算复杂。
(3)阻尼法
将损失函数的二阶泰勒展开记作,求以下函数的最小化
其中,μ≥0为阻尼因子,是惩罚项,对新的损失函数求一阶导,并令其等于0有:
(4)非线性最小二乘法
将残差组合成向量的形式:,则有
,同理,记
,则有
残差函数f(x)为非线性函数,对其一阶泰勒近似有:,J是残差函数f的雅可比矩阵
代入损失函数,
(7)
这样损失函数近似成了一个二次函数,并且如果雅克比是满秩的,则正定,损失函数有最小值。
且,,
F'(x)和F''(x)证明,用导数的定义即可
(5)高斯-牛顿法和LM算法
高斯-牛顿法:令公式7的一阶导数为0,得到,该式子就是
,称为normal equation
列文伯格马夸尔特方法,对高斯牛顿法进行了改进,在求解过程中引入了阻尼因子,μ≥0
阻尼因子的作用?
1) μ>0保证了
正定,迭代朝着下降方向进行
2) μ非常大,则
,接近最速下降法
3) μ比较小,则
,接近高斯牛顿法
(6)鲁棒核函数的实现
鲁棒核函数直接作用残差fk(x)上,最小二乘函数变成了如下形式
,将误差的平方项记作
,将鲁棒核函数进行二阶泰勒展开有
,上述函数的中的
的计算稍微复杂一点:
代入上式
对上式求和后,对变量△x求导,令其等于0,得到
柯西鲁棒核函数的定义为:
,其中c为控制参数,对s的一阶导和二阶导为
,
回顾
最小二乘法求解步骤:
1. 找到一个合适的关于状态量x的残差函数fi(x),后续用r,err等表示
2. 计算残差函数对状态量x的雅克比J
3. 选定cost function以及及其参数
4. LM算法求解