从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)

213
0
2020年10月3日 17时23分

分上下篇吧,一共有七八十个公式呢 ^-^ 写在一篇就太长了

 

1

 

大批公式预警!

 

2

 

基于 Bundle Adjustment 的 VIO 融合

视觉 SLAM 里的 Bundle Adjustment 问题

↓下图选自 SBA: A Software Package for Generic Sparse Bundle Adjustment [1],g2o和ceres都与SBA紧密联系,感兴趣的可以看下reference里面的论文。

 

3

 

已知 1) 状态量初始值(特征点的三维坐标,相机的位姿);2) 系统测量值(特征点在不同图像上的图像坐标)。构建误差函数,利用最小二乘得到状态量的最优估计:

 

4

 

[公式] 是旋转四元数; [公式] 是平移向量; [公式] 是特征点 3D 坐标; [公式] 是第 i 个相机坐标系; [公式] 是投影函数; [公式] 是 [公式] 对 [公式] 的观测; [公式] 是 [公式] 范数。

 

最小二乘问题的求解

最小二乘基础概念

找到一个 [公式] 维的变量 [公式] ,使得损失函数 [公式] 取局部最小值:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(16)

其中 [公式] 是残差函数,比如测量值和预测值之间的差,且有 [公式] 。局部最小值指对任意 [公式] 有 [公式] 。

假设损失函数

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(21)
(2)

其中 [公式] 和 [公式] 分别为损失函数 [公式] 对变量 [公式] 的一阶导和二阶导矩阵(Jacobian 和 Hessian)

忽略泰勒展开的高阶项,损失函数变成了二次函数,可以得到如下性质:

如果在点 [公式] 处有导数为 0,则称这个点为稳定点。在点 [公式] 处对应的 Hessian 为 [公式] :

  • 如果是正定矩阵,即它的特征值都大于0,则在 [公式] 处有 [公式] 为局部最小值
  • 如果是负定矩阵,即它的特征值都小于0,则在 [公式] 处有 [公式] 为局部最大值
  • 如果是不定矩阵,即它的特征值大于0也有小于0的,则 [公式] 处为鞍点

迭代下降法求解:下降法

找一个下降方向使损失函数随 [公式] 的迭代逐渐减小,直到 [公式] 收敛到 [公式] :

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(29)

分两步:1)找下降方向的单位向量 [公式] ;2)确定下降步长 [公式] 。

假设 [公式] 足够小,那么对损失函数 [公式] 进行一阶泰勒展开:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(32)

只需寻找下载方向,满足:[公式]

通过 line search 方法找到下载的步长:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(34)

最速下降法:适用于迭代的开始阶段

从下降方向的条件可知: [公式] , [公式] 表示下降方向和梯度方向的夹角。当 [公式],有 [公式] ,即梯度的负方向为最速下降方向。缺点是:最优值附近震荡 zigzag,收敛慢。

牛顿法:适用于最优值附近

在局部最优点 [公式] 附近,如果 [公式] 是最优解,则损失函数对 [公式] 的导数等于 0,对公式(2)取一阶导有:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(41)
(3)
从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(42)

得到: [公式] 。缺点:二阶导矩阵计算复杂。

阻尼法 Damp Method

将损失函数的二阶泰勒展开记作:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(44)

求以下函数的最小化:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(45)

其中, [公式] 为阻尼因子, [公式] 是惩罚项,目的是限制 [公式] 使它不能过大。

对新的损失函数求一阶导,并令其等于 0 有:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(48)
(4)

符号说明

为了公式约简,可将残差组合成向量的形式:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(49)
(5)

则有:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(50)

同理,如果记 [公式] 则有:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(52)
(6)

高斯牛顿法 Gauss-Newton Method

残差函数 [公式] 为非线性函数,对其一阶泰勒近似有:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(54)

带入损失函数:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(55)
(7)

这样损失函数就近似成了一个二次函数,而且如果雅可比是满秩的,则 [公式] 正定,损失函数有最小值(半正定会使得 [公式] 有非零 [公式] 使得等式成立,而正定会仅有 [公式] )。另外,有: [公式] ,以及 [公式]

令公式(7)的一阶导等于 0,得到:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(61)
(8)

上式就是通常论文中看到的 [公式] ,称其为 normal equation

LM – 列文伯格马夸尔特算法(The Levenberg-Marquardt Method)

对高斯牛顿法进行了改进,求解过程中引入了阻尼因子:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(63)

阻尼因子的作用

  • [公式] 保证 [公式] 正定,迭代朝着下降方向进行。
  • [公式] 非常大,则 [公式] ,接近最速下降法。
  • [公式] 比较小,则 [公式] ,接近高斯牛顿法。

阻尼因子初始值的选取

阻尼因子 [公式] 大小是相对于 [公式] 的元素而言的。半正定的信息矩阵 [公式] 特征值 [公式] 和对应的特征向量 [公式] 。对 [公式] 做特征值分解后有: [公式] 可得:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(72)
(9)

所以,一个简单的 [公式] 初始值的策略就是:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(74)

通常,按需设定 [公式] ,由于每次求算 [公式] 的特征值的最大值添加了很大计算量。那么 [公式] 即取出 [公式] 对角线上的最大值,其与特征值的最大值同一数量级。

阻尼因子 [公式] 的更新策略

定性分析

  1. 如果 [公式] ,则 [公式] ,增大阻尼减小步长,拒绝本次迭代。
  2. 如果 [公式] ,则 [公式] ,减小阻尼增大步长,加快收敛,减少迭代次数。

定量分析

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(81)
(10)

此公式表示 实际下降/近似下降

其中:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(82)
(11)检查发现这里有点错误,倒数第二行应该是 -2b+b-后面这项,是-不是+

这也是 SBA 控制因子 [公式] 的来源。

Marquardt 策略

首先比例因子分母始终大于 0,如果:

  • [公式] ,则 [公式] ,应该 [公式] ,增大阻尼减小步长。
  • 如果 [公式] 且比较大,减小 [公式] ,让 LM 接近 Gauss-Newton 使得系统更快收敛。二阶收敛可以理解为用二次函数拟合曲线。
  • 反之,如果是比较小的正数,则增大阻尼 [公式] ,缩小迭代步长。

1963年 Marquardt 提出了如下的阻尼策略:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(88)
(12)

Nielsen 策略(被 g2o,ceres 采用)

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(89)
(13)

其实就是在下降时,如果实际误差变化小于模型误差, [公式] 会取到 [公式] 变大,使得步长小一点;如果实际误差变化大于模型误差, [公式] 会取到 [公式] ,步长大一点更快收敛。当误差上升,因为 [公式] 的分母是 [公式] 的,会使 [公式] 而且如果持续上升就会一直以指数增大,使得步长很小,放弃该步拟合。

鲁棒核函数的实现

引言:最小二乘遇到 outlier 怎么处理?核函数如何在代码中实现?有很多方法[2],这里介绍 g2o 和 ceres 中使用的 Triggs Correction[3]

鲁棒核函数直接作用残差 [公式] 上,最小二乘函数变成了如下形式:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(95)

将误差的平方项记作 [公式] ,则鲁棒核误差函数进行二阶泰勒展开有:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(97)
(14)

上述函数中 [公式] 的计算稍微复杂一点:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(99)
(15)

公式(15)代入公式(14)有:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(100)
(16)

对公式(16)求和后,对变量 [公式] 求导,令其等于 0,得到:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(101)
(17)

Example: Cauchy Cost Function [公式] IRLS

柯西鲁棒核函数的定义为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(103)

其中 [公式] 为控制参数。对 [公式] 的一阶导和二阶导为:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(106)

Huber 核:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(107)

核函数控制参数的设定

95% efficiency rule (Huber,1981) : it provides an asymptotic efficiency 95% that of linear regression for the normal distribution.[4]

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(108)
  • 如果残差 [公式] 是正态分布,Huber [公式] ,Cauchy [公式] 。
  • 如果残差非正态分布,需估计残差方差,然后对残差归一化。median absolute residual 方法 [公式] 。

这里正态分布是 DVO、SVO等算法早期把所有残差项都拿出来做一个统计,求出一个近似的方差,然后通过这个方差做一个 robust kernel 的处理。

Vector3D rho; // 用来保存鲁棒核函数,一阶导,二阶导
// rho[0] = rho(sq_norm), //sq_norm 均方差指的是f^T*f
// rho[1] = rho'(sq_norm),
// rho[2] = rho''(sq_norm),
this->robustKernel()->robustify(error,rho);
InformationType weightedOmega = this->robustInformation(rho);
omega_r *= rho[1]; // 公式中的 rho'(r^2) * r。omega_r就是r,*=一个rho的一阶导rho'

from->b().noalias() += A.transpose() * omega_r; // 公式中的 b = -rho'(r^2) * r^T * J。A是雅可比J
// noalias()用来解决混淆问题,alias在英语里是别名的意思
from->A().noalias() += A.transpose() * weightedOmega * A; // 公式中的 J^T * W * J

代码中,基本和前段的(17)推导一致,robustinformation()函数在base_edge.h中进行了实现:

从零手写VIO——(三)基于优化的 IMU 与视觉信息融合(上篇)插图(112)
InformationType robustInformation(const Vector3D& rho)
{
    // _information 可以k看做单位矩阵
    InformationType result = rho[1] * _information;
    // 计算权重 w = rho' + 2 * rho'' * r * r^T
    // 但是不知道作者为啥注释了这段代码,也就是变成了 w = rho' 应该是把二阶近似省略了
    //ErrorVector weightedError = _information * _error;
    //result.noalias() += 2 * rho[2] * (weightedError * weightedError.transpose());
    return result;
}

参考

  1. ^SBA: A Software Package for Generic Sparse Bundle Adjustment http://users.ics.forth.gr/~lourakis/sba/sba-toms.pdf
  2. ^Zach C . Robust Bundle Adjustment Revisited[J]. 2014. http://vigir.missouri.edu/~gdesouza/Research/Conference_CDs/ECCV_2014/papers/8693/86930772.pdf
  3. ^Bill Triggs et al. “Bundle adjustment – a modern synthesis”. In: International workshop on vision algorithms. Springer. 1999. pp. 298-372. https://www.cct.lsu.edu/~kzhang/papers/BundleAdjustment.pdf
  4. ^Kirk MacTavish and Timothy D Barfoot. “At all Costs: A Comparison of Robust Cost Functions for Camera Correspondence Outliers”. In: 2015 12th Conference on Computer and Robot Vision. IEEE, 2015, pp 62-69. http://ncfrn.mcgill.ca/members/pubs/AtAllCosts_mactavish_crv15.pdf

 

 

发表评论

后才能评论