目录
1.预处理部分(第4页):
1.1利用KLT方法[29]跟踪角点[30],提取的时使特征均匀分布,利用RANSAC基础矩阵剔除外点。
1.2关键帧筛选策略:1)计算当前帧相对最近的关键帧的特征平均视差,如果视差大于一个阈值就为关键帧。因为视差可以根据平移和旋转共同得到,而纯旋转则导致不能三角化成功,所以这一步需要IMU预积分的旋转进行补偿。2)如果当前帧跟踪到的特征点数目小于一定阈值,则选为关键帧。
1.3IMU预积分:
式子(4)先给出积分连续状态下的积分形式(在世界坐标系下),四元数相乘的方程比较特殊(如(3)式第三项)。然后在(5)(6)式中,给出预积分的连续形式(6)式左面的 等于 (5)式右面的。预积分是连续两帧之间通过IMU测量量积分获得,因为与上一状态无关,所以计算简单,研究人员经常使用。当偏置变化比较小的时候,利用对两个偏置的一阶偏导为雅克比更新
的值。式子(7)给出离散欧拉积分的预积分方程。式子(9)给出IMU预测状态量的误差的传递方程,目的是可以从当前时刻IMU状态误差传递到下一时刻IMU状态误差。
VINS技术路线中给出IMU预积分部分的雅克比,这个雅克比矩阵一方面用于预积分的更新。如式子(12)另一方面可以计算预积分测量值的协方差。重点:式子(9)推导连续状态下IMU预积分状态导数对IMU bk状态的雅克比目的是接下来推到离散状态下,IMU预积分状态对IMUb k状态的雅克比。第二个雅克比才有用。
2.初始化部分(第6页)
2.1.纯视觉初始化用迭代初始量(本质矩阵或单应矩阵)的方法初始化,VINS初始化对齐,一般对齐尺度,重力,速度,和bias。VINS本文初始化过程中忽视掉了加速度计的bias,因为加速度计与重力耦合,并且重力向量很大,初始化过程动态过程很短,幅度又不大,加速度计偏置很难观测到。而陀螺仪的偏置,是通过将IMU放在桌子上读数很容易得到。
2.2具体初始化过程:
选择一个滑动窗口,在最后一帧和滑窗中寻找,跟踪到的点数目大于30个的并且视差超过20的(经过旋转补偿后的),如果找到这帧,就进行5点法本质矩阵初始化。之后随意设置一个尺度因子,三角化这两帧所跟踪到的所有特征点,之后用PNP方法,估计滑窗中的每一帧位姿了,之后采用全局BA(Full BA)重投影误差的方式优化所有帧的位姿。将视觉获得的位姿,假设IMU-Camera外参已知,转换到IMU坐标系下。然后与IMU预积分的两帧之间的旋转做BA 优化。优化得到陀螺仪的bias。得到陀螺仪的bias之后,重新进行预积分,重新计算所有两帧之间的IMU预积分。
2.3初始化速度,重力向量,尺度:通过HX=B 利用cholesky分解获得(这里有些像后端优化部分,只是变换了形式,将物理模型中的IMU状态改成相机状态(通过纯视觉估计 再利用外参变换的),利用物理模型和IMU预积分求一个残差,利用cholesky分解,便于求解)其中重力向量的模已知,所以在切平面上用两个正交基描述。获得重力向量在c0坐标系下的向量gc0后,通过将重力旋转到Z轴上,从而得到两者的旋转,之后把所有的变量都变换到世界坐标系上。
3.紧耦合优化部分(第7页)
3.1IMU残差,就是用物理模型得到的(里面值用视觉和初始化获得的)减去IMU预积分得到的。优化变量就是IMU时刻下的位置,速度,旋转,两个偏置。
3.2视觉残差,在相机的归一化平面上比较残差。将视觉残差投影到正切平面上。
3.3边缘化:分为两种情况,一种是临近的也就是倒数第二帧如果是关键帧的话,就把第一个关键帧(最老的)对IMU预积分约束和视觉观测点进行边缘化;如果倒数第二帧不是关键帧的话,那么就只剔除倒数第二帧的视觉观测,保留它的IMU数据,目的是保证了IMU预积分的连贯性。原因是边缘化保证关键帧之间有足够视差而能够三角化足够多的地图点。
3.4Motion-only VI BA:不是优化滑动窗口中的每一个帧,而是只优化最近固定的几个IMU状态,并且只优化位置,姿态和速度。特征的深度,外参,bias和老的IMU状态都不参与优化,而是作为一个常量。因为采用这个方案,VINS一般能够达到30HZ。
3.5IMU前向传播:通过IMU前向传播策略可以达到IMU速率的输出(100HZ),因为VIO速率受视觉限制不会很快。目的,1.可以用在回环部分的状态反馈,2.可以用在自主飞行的实时反馈.
3.6误差检测和恢复:1)如果跟踪到的特征数目变的很少
2)IMU和视觉估计的状态有较大不连续性
3)估计的bias和外参变化都很大
如果遇到上述的三种情况,则系统选择重新回到初始化状态,系统重新运行。
4.重定位部分(第9页)
4.1选择500个角点用BRIEF描述子描述,用词带模型来检测闭环,经过时间和空间一致性检验后返回回环候选帧。因为VIO可以提供滚转和俯仰角,所以不依赖旋转不变性。
如果检测到回环,那么回环约束会加入到pose graph中,pose graph中的回环约束是4个自由度的相对刚体变换。pose graph中包括所有相关连变换约束。pose graph优化在新的线程中运行。
4.2重检索特征:当前滑窗中的帧和回环帧进行特征关联,通过描述子匹配,直接匹配会有很多外点,所以采用1.两帧之间用RANSAC 基础矩阵剔除外点。2.利用当前滑窗中的3D点和回环帧的2D对应特征,进行PnP测验,如果内点数目大于阈值,则进行重定位,重定位过程和Pose Graph优化分别在两个独立的线程运行。
4.3紧耦合重定位:重定位的误差函数和后端VIO优化几乎一样,只是在后面添加了当前滑窗和回环候选帧的约束,这个额外约束用视觉残差来表示。这样就可以将滑窗全部“拽”到回环帧上。这步优化的过程中,回环帧的位姿是常量,保持不变。全局的优化在重定位之后才开始做。
5.全局Pose Graph 优化
全局Pose Graph主要只优化四个自由度(yaw,x,y,z)
5.1当一个关键帧从滑窗中边缘化出来后,就加入到了Pose Graph中,这个关键帧作为Pose Graph中的一个顶点。这个顶点在Pose Graph中可能有两种边,一种是连续边:从VIO计算边(注意这里是相邻的两个关键帧之间的变换 做 残差)。一种是回环边:从重定位获得边,两者边的的残差定义一样如公式(28)。
5.2Pose Graph 管理:随着时间推移和不断运动,Pose Graph会增长的越来越多。所以对Pose Graph进行降采样,含有回环边的关键帧不会被降采样,其他离的很近关键帧并且姿态很相近的关键帧就会被剔除掉。