VINS理论与代码详解5——基于滑动窗口的单目视觉紧耦合后端优化模型
VINS理论与代码详解5——基于滑动窗口的单目视觉紧耦合后端优化模型windows
终于讲到了真正的视觉惯性紧耦合系统了，（在这里插一句，算是写博客和学习一个SLAM系统的心得吧，一开始咱们可能抱着一个必胜的心态打算征服全部的知识点和难点，但到了整个过程的中期，总会出现或多或少的问题，阻碍着咱们前进的步伐，你可能想放松警戒，不想太深究具体的公式和内容，急功心切的想看到终点究竟是什么，可是我要提醒你们，也包括本身，最美好的以及最有价值的永远是过程，是在过程当中学到的能够为一辈子所用的经验，只是一味着冲到终点，而忘记了过程，你最终会发现获得的只有失望。若是你不想失望的话，那就再接再砺，给本身继续加油吧！）到这里才是整个系统的重点，前面全部说起的其实都是松耦合方式，目的是给整个系统提供优化初值和状态。视觉惯性紧耦合优化模型在第VI部分，我理解的紧耦合系统是将视觉和惯性的原始观测量进行有效的组合，也就是在数据处理前就进行数据融合，VINS中是将滑动窗口内的状态量整合到一个状态向量中，如公式21所示：后端
第一个式子是滑动窗口内整个状态向量，其中n是帧数，m是滑动窗口中的特征点总数，维度是15*n+6+m，第二个式子xk是在第k帧图像捕获到的IMU状态，包括位姿，速度，旋转，加速度计和陀螺仪偏置，第三个式子是相机外参，λ是特征点深度值得逆。从状态向量就能够看出xk只与IMU项以及Marginalization有关，特征点的深度值只与camera和Marginalization有关，因此下面创建BA紧耦合模型的时候按照这个思路，下面创建一个视觉惯性的BA优化模型以及鲁棒核函数模型：框架
从公式中能够明显的看出来，BA优化模型被分红了三个部分，分别是Marginalization（边缘化）残差部分（从滑动窗口中去掉的位姿和特征点的约束），IMU残差部分（滑动窗口中相邻帧间的IMU产生）和视觉代价偏差函数部分（滑动窗口中特征点在相机下的投影产生），其中鲁棒核函数针对代价函数设定。下面具体介绍着三个部分。函数
（1） IMU观测值残差学习
考虑到的是两个连续帧bk和bk+1之间的观测值，与前面的预积分模型相同，如公式24所示，还记得在IMU预积分的时候求获得协方差矩阵和观测值吗？那里求得的观测值就是测量值，协方差矩阵就是这里的协方差矩阵，而公式24前面的项就是预测的值，也称估计值，估计值和测量值之间的差值就是残差，其实这里的公式24是由公式5的右边左移获得，你发现了吗？优化
（2） 视觉观测值残差spa
与传统的针孔相机模型定义的重投影偏差不一样，论文中使用的是单位半球体的相机观测残差，是一个宽视野鱼眼或者全方位相机。相机的残差模型以下公式25所示：.net
第一个式子就是残差的表达式，第二个式子是鱼眼相机反投影函数将观测到的像素坐标转换成单位向量的观测值数据，b1和b2是此单位向量的切平面上的一组基。第三个式子是重投影估计模型。3d
（3） Marginalizationblog
这一部分借鉴博客：http://blog.csdn.net/q597967420/article/details/76099443
sliding windowsbounding边界化了优化问题中pose的个数, 从而防止pose和特征的个数随时间不断增长, 使得优化问题始终在一个有限的复杂度内, 不会随时间不断增加。然而, 将pose移出windows时, 有些约束会被丢弃掉, 这样势必会致使求解的精度降低, 并且当MAV进行一些退化运动(如: 匀速运动)时, 没有历史信息作约束的话是没法求解的. 因此, 在移出位姿或特征的时候, 须要将相关联的约束转变成一个约束项做为prior放到优化问题中. 这就是marginalization要作的事情。
VINS-MONO中,为了处理一些悬停的case,引入了一个two-way marginalization, 简单来讲就是:若是倒数第二帧是关键帧, 则将最旧的pose移出slidingwindow, 也就是MARGIN_OLD,若是倒数第二帧不是关键帧, 则将倒数第二帧pose移出sliding window, 也就是MARGIN_NEW。选取关键帧的策略是视差足够大，在悬停等运动较小的状况下, 会频繁的MARGIN_NEW, 这样也就保留了那些比较旧可是视差比较大的pose. 这种状况若是一直MARGIN_OLD的话, 视觉约束不够强, 状态估计会受IMU积分偏差影响, 具备较大的累积偏差。
到这里总体VINS理论的框架基本上算是介绍完毕了，对于后面的重定位，全局位姿优化和回环检测等有时间再作下讨论，如今有没有一个比较清晰的思路？若是还比较困惑的话，那就赶忙进行下面的代码实战环节吧。下面就从代码角度来分析VINS的整个第二部分。
（4）单目视觉雅可比计算（参考贺大佬）
视觉SLAM十四讲中第162页有提到，将空间坐标系下的3D点投影到像素坐标系下作最小二乘，求出该最小二乘模型的雅各比矩阵既能够获得高斯牛顿或者LM方法的降低梯度，从而优化位姿和路标点，具体的内容请参照书本。须要注意的是VINS在计算视觉雅各比时与书上有三点不一样：
①VINS中由feature_tracker传过来的像素点坐标吸取了内参K且作了归一化，所以是吸取内参的归一化图像坐标。
②VINS中使用Q旋转四元数来优化位姿，所以在计算位姿对应的雅各比矩阵时和书上的公式不彻底相同。
③VINS是基于滑动窗口，优化的位姿包含滑动窗口中的11帧位姿，但同时是依靠特征点来肯定所优化哪两帧的位姿，每次传入视觉残差模型是遍历特征点来肯定的，设定i帧为最开始观测到次特征点的相机，则j（不等于i）就为其余共视关键帧。
4.1视觉j帧残差计算（其实也能够计算i帧残差，过程基本类似）
明白上面三个不一样点就能够进行下面的公式推导了，首先下图是残差计算模型，VINS选取的是j帧图像的残差计算模型。
4.2总体雅各比公式
传入到上面的j帧视觉残差模型的优化状态量分别是7维的i帧位姿，7维的j帧位姿，7维的相机和IMU外参数以及1为的特征点逆深度。所以须要计算的雅各好比下图所示：
VINS系统的全部结果的参考坐标都是世界坐标，同时都是对应IMU时刻在世界坐标系下的旋转，平移，速度等，并且逆深度的值时相对于i时刻的相机，所以咱们须要整理4.1获得的i时刻的关键帧对应到j时刻的归一化相机坐标Pj的重投影方程以下图所示：（咱们须要求的是Pj对于i、j时刻关键帧，相机外参以及i时刻的逆深度，因此须要构建这样一个重投影方程，才能知足偏导数求解）
由最终的公式能够看到Pj由i，j时刻的位姿（旋转和平移），相机外参（旋转和平移）以及逆深度表示，下面能够直接求雅各比（偏导数）。
4.3视觉i帧的状态量（位移和旋转）在Pj下的雅各比
这里和视觉SLAM十四讲中有出入，可是解算原理基本相同，利用了旋转四元数和差乘的性质。获得下面的偏导后整个J[0]就能够相乘获得了。
4.4视觉j帧的状态量、相机和IMU之间外参数（位移和旋转）在Pj下的雅各比
这里的雅各比推导过程就不重复了，和上面的过程类似，就直接给结果了
4.6特征点你深度在Pj下的雅各比
逆深度求偏导就比较简单了，就是一个反函数求偏导过程，获得下面的偏导后整个J[3]就能够相乘获得了。
至此，整个基于滑动窗口的视觉残差和雅各比模型就完成了。其协方差矩阵较为简单，代码中直接赋值便可，不须要计算。