本文深入解析VINS-Mono的后端非线性优化，涉及视觉和IMU的残差函数、Jacobian和协方差计算，以及基于舒尔补的边缘化技术，旨在优化滑动窗口中所有帧的PVQ、bias等状态量。
本讲是VINS最核心部分了，前面经历了
1）视觉跟踪feature_tracker、IMU预积分integrationBase类；
2）初始化中SFM纯视觉估计滑动窗中所有帧的位姿和3D路标点深度、SFM与IMU预积分松耦合对齐求解初始化参数。
在完成前面的初始化环节后，本节将进行第3部分，基于滑动窗的紧耦合后端非线性优化：将视觉约束、IMU约束、闭环约束放到一个大的目标函数中进行非线性优化，求解出滑动窗口中所有帧的PVQ、bias等。
在视觉约束和IMU约束中，基本思想是找到优化状态向量，然后通过视觉残差和IMU测量残差分别对状态向量求导，获得视觉和IMU预积分的Jacobian和协方差矩阵。
阅读本文所需知识储备：
2、《视觉SLAM十四讲精品总结》4：非线性优化g2o(BA基础)
目录
一、VIO残差函数的构建
1、需要优化的状态向量：
滑动窗口内IMU状态（PVQ、加速度bias、陀螺仪bias）、IMU到Camera的外参、m+1个3D路标点逆深度。
第一个式子是滑动窗口内所有状态量，n是关键帧数量，m是滑动窗内所有观测到的路标点总数，维度是15*n+6+m。特征点逆深度为了满足高斯系统。
第二个式子xk是在第k帧图像捕获到的IMU状态，包括位置，速度，旋转（PVQ）和加速度偏置，陀螺仪偏置。
第三个式子是相机外参。
注意：xk只与IMU项和Marg有关；特征点深度也只与camera和Marg有关；
2、目标函数为：
视觉惯性BA：这三项分别为边缘化的先验信息、IMU的测量残差、视觉的重投影误差
BA优化模型分为三部分：
1、Marg边缘化残差部分（滑动窗口中去掉位姿和特征点约束）代码中使用Google开源的Ceres solver解决。
2、IMU残差部分（滑动窗口中相邻帧间的IMU产生）
3、视觉误差函数部分（滑动窗口中特征点在相机下视觉重投影残差）
二、视觉约束（细致）
这部分要拟合的目标可以通过重投影误差约束，求解的是对同一个路标点的观测值和估计值之间的误差，注意是在归一化平面上表示。
1.视觉重投影误差residual（归一化平面）
相关自变量是：该路标点被两帧观察到的相机帧位姿、Cam和IMU之间相对位姿、路标点在第一帧的逆深度。
当某路标点在第i帧观测到并进行初始化操作得到路标点逆深度，当其在第j帧也被观测到时，估计其在第j帧中的坐标为：
此时的视觉残差为：（左侧为根据i帧反推估计的位置，右侧为观测值）
逆深度参数化
逆深度作为参数原因：1）观测到的特征点深度可能非常大，难以进行优化；2）可以减少实际优化的参数变量；3）逆深度更加服从高斯分布。这里特征点的逆深度在第i帧初始化操作中得到。
论文中：
上面的公式是传统的针孔相机模型，这里用的是单位半球体的相机观测残差。是一个鱼眼相机。
在相机的归一化平面上比较残差，再将视觉残差投影到单位球面的正切平面上。由于视觉残差的自由度是2，所以我们将残差向量投影到切平面上。
在第i帧第一次观测到第l个路标点，在第j帧中对该路标点进行观测的残差为：
第一个式子就是残差的表达式，第二个式子是鱼眼相机反投影函数将观测到的像素坐标转换成单位向量的观测值数据，b1和b2是此单位向量的切平面上的一组基。第三个式子是重投影估计模型。其实VINS代码中也可以使用普通的针孔相机模型。
2、优化变量
包括两个时刻的状态量、外参，以及逆深度
值得注意的是：和纯视觉不同的是，待优化的状态量中的旋转平移都是IMU系到w系，而不是Camera到w系。
状态量特征：
1、待优化量和IMU约束中待优化量有重叠，再次体现了紧耦合。
2、和IMU不同的是，IMU每次优化的状态量是相邻两帧的，但是视觉优化的2帧不一定是相邻的，因此用i，j表示。
3、Jacobian
求解Jacobian就是用视觉残差对上述7个状态变量进行求导。
方法一：崔博直接给出答案
最低0.47元/天 解锁文章
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
