IMU与视觉信息融合—手写VIO课程笔记2（上）

最新推荐文章于 2023-07-21 21:40:47 发布

原创最新推荐文章于 2023-07-21 21:40:47 发布 · 632 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#slam

IMU 专栏收录该内容

4 篇文章

订阅专栏

本文探讨了视觉SLAM中的Bundle Adjustment（BA）问题，涉及状态量优化，包括特征点三维坐标和相机姿态的估计。同时，深入讲解了最小二乘问题在VIO信息融合中的应用，介绍了迭代下降法、最速下降法、牛顿法、阻尼法、高斯-牛顿法和LM算法，以及鲁棒核函数在求解过程中的实践。

一、基于BA的VIO融合

1.视觉SLAM中的BA问题

已知：状态量初始值（特征点的三维坐标，相机的位姿），系统测量值（特征点在不同图像上的图像坐标）

问题：如何估计状态量的最优值？

解决方式：构建误差函数，利用最小二程得到状态量的最优估计

q：旋转四元数 p：平移向量 f：特征点3D坐标，q p f本身是有下标的

$c_i$ ：第i个相机系， $\pi$ (·)：投影函数，三维空间的点投影到相机平面上，如果是使用相机的像素坐标即为二维的，本次课程主要使用的是归一化平面（z=1）上的坐标

$z_f_j^c^i$ ： $c_i$ 对 $f_j$ 的观测， $\sum _i_j$ ：范数

2.VIO信息融合问题

IMU误差 $z_b_1_b_2$ 是通过IMU预积分构建的，在两幅图像之间会存在很多个IMU测量值，所以把这些进行积分得到一个约束

二、最小二乘问题的求解

1.最小二乘基础概念

定义：找到一个n维的变量 $x\in R^n$ ,使得损失函数F(x)取局部最小值： $F(x) = \cfrac{1}{2}\sum _{i=1}^m(f_i(x))^2$ ,其中fi是残差函数，比如测量值和预测值之间的差，且有m大于等于n。

局部最小值指对任意 $\parallel x - x^*\parallel <\delta$ 有 $F(x^*)\leq F(x)$

损失函数泰勒展开：假设损失函数F(x)可导并且平滑的，因此，二阶泰勒展开：

$F(x+\Delta x)=F(x)+J\Delta x+\cfrac{1}{2}\Delta x^TH\Delta x+O(\parallel \Delta x\parallel ^3)$

其中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*： $F(x_{k+1})< F(x_k)$

第一，找到下降方向单位向量d，第二，确定下降步长α，假设α足够小，我们可以对损失函数F(x)进行一阶泰勒展开：

$F(x+\alpha d)\approx F(x)+\alpha Jd$ ，只需寻找下降方向，满足Jd<0，通过line search方法找到下降的步长 $\alpha ^*=argmin_{\alpha >0}\left \{ F(x+\alpha d) \right \}$

line search，线搜索方法，是一种迭代的求得某个函数的最值得方法，对于每次迭代，线搜索会计算得到搜索的反向pk以及沿这个方向移动的步长αk.

（2）最速下降法（适用于迭代的开始阶段），从下降方向的条件可知： $Jd=\parallel J\parallel cos\theta$ , $\theta$ 表示下降方向和梯度方向的夹角，当 $\theta =\pi$ ，有 $d=\cfrac{-J^T}{\parallel J\parallel }$ ,即梯度的负方向为最速下降方向。缺点：最优值附近震荡，收敛慢

牛顿法（适用于最优值附近），在局部最优点x*附近，如果x+△x是最优解，则损失函数对△x的导数等于0，对公式 $F(x+\Delta x)=F(x)+J\Delta x+\cfrac{1}{2}\Delta x^TH\Delta x+O(\parallel \Delta x\parallel ^3)$ 取一阶导数有

$\cfrac{\partial }{\partial \Delta x}(F(x)+J\Delta x+\cfrac{1}{2}\Delta x^TH\Delta x)=J^T+H\Delta x=0$ ,可以得到 $\Delta x=-H^{-1}J^T$ ,缺点为二阶导矩阵计算复杂。

（3）阻尼法

将损失函数的二阶泰勒展开记作 $F(x+\Delta x)\approx L(\Delta x)\equiv F(x)+J\Delta x+\cfrac{1}{2}\Delta x^TH\Delta x$ ，求以下函数的最小化 $\Delta x\equiv arg min_{\Delta x}\left \{ L(\Delta x)+\cfrac{1}{2}\mu \Delta x^T\Delta x \right \}$

其中，μ≥0为阻尼因子， $\cfrac{1}{2}\mu \Delta x^T\Delta x=\cfrac{1}{2}\parallel \Delta x\parallel ^2$ 是惩罚项，对新的损失函数求一阶导，并令其等于0有：

$L'(\Delta x)+\mu \Delta x = 0\Rightarrow (H+\mu I)\Delta x = -J^T$

（4）非线性最小二乘法

将残差组合成向量的形式： $f(x)=\begin{bmatrix} f_1(x)\\ ...\\ f_m(x) \end{bmatrix}$ ，则有 $f^T(x)f(x)=\sum _{i=1}^m(f_i(x))^2$ ,同理，记 $J_i(x)=\cfrac{\partial f_i(x)}{\partial x}$ ，则有 $\cfrac{\partial f(x)}{\partial x}=J=\begin{bmatrix} J_1(x)\\ ...\\ J_m(x) \end{bmatrix}$

残差函数f(x)为非线性函数，对其一阶泰勒近似有： $f(x+\Delta x)\approx \l (\Delta x)\equiv f(x)+J\Delta x$ ，J是残差函数f的雅可比矩阵

代入损失函数， $F(x+\Delta x)\approx L(\Delta x)\equiv \cfrac{1}{2}\l (\Delta x)^2\l (\Delta x) =\cfrac{1}{2}f^Tf+\Delta x^TJ^Tf+\cfrac{1}{2}\Delta x^TJ^TJ\Delta x$

$=F(x)+\Delta x^TJ^Tf+\cfrac{1}{2}\Delta x^TJ^TJ\Delta x$ （7）

这样损失函数近似成了一个二次函数，并且如果雅克比是满秩的，则 $J^TJ$ 正定，损失函数有最小值。

且， $F'(x)=(J^Tf)^T$ ， $F''(x)\approx J^TJ$

F'(x)和F''(x)证明，用导数的定义即可 $F'(x)=\lim_{\Delta x\rightarrow 0}\cfrac{F(x+\Delta x)-F(x)}{\Delta x}$

（5）高斯-牛顿法和LM算法

高斯-牛顿法：令公式7的一阶导数为0，得到 $(J^TJ)\Delta x_g_n=-J^Tf$ ,该式子就是 $H\Delta x_g_n=b$ ，称为normal equation

列文伯格马夸尔特方法，对高斯牛顿法进行了改进，在求解过程中引入了阻尼因子 $(J^TJ+\mu I)\Delta x_l_m=-J^Tf$ ,μ≥0

阻尼因子的作用？

1) μ>0保证了 $(J^TJ+\mu I)$ 正定，迭代朝着下降方向进行

2) μ非常大，则 $\Delta x=-\cfrac{1}{\mu}J^Tf=-\cfrac{1}{\mu}F'(x)^T$ ，接近最速下降法

3) μ比较小，则 $\Delta x_l_m\approx \Delta x_g_n$ ,接近高斯牛顿法

（6）鲁棒核函数的实现

鲁棒核函数直接作用残差fk(x)上，最小二乘函数变成了如下形式

$min_x\cfrac{1}{2}\sum _k\rho (\parallel f_k(x)\parallel ^2)$ ，将误差的平方项记作 $s_k=\parallel f_k(x)\parallel ^2$ ,将鲁棒核函数进行二阶泰勒展开有

$\cfrac{1}{2}\rho (s)=\cfrac{1}{2}(const+p'\Delta s+\cfrac{1}{2}\rho ''\Delta ^2s)$ ，上述函数的中的 $\Delta s_k$ 的计算稍微复杂一点：

代入上式

对上式求和后，对变量△x求导，令其等于0，得到

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

$\rho (s)=c^2log(1+\cfrac{s}{c^2})$ ,其中c为控制参数，对s的一阶导和二阶导为

$\rho '(s)=\cfrac{1}{1+\cfrac{s}{c^2}}$ ， $\rho ''(s)=-\cfrac{1}{c^2}(\rho '(s))^2$