VINS代码笔记

80 阅读 0 评论 53 点赞

我是靠谱客的博主碧蓝火龙果，最近开发中收集的这篇文章主要介绍VINS代码笔记，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

estimator_node.cpp系统入口

首先找到main函数,读取参数,设置参数,

imu_callback:

 IMU测量的回调函数

 imu_msg [接受到的IMU消息]

1. 执行con.notify_one();唤醒作用于process线程中的获取观测值数据的函数(怎么涉及到process线程的?)

2. predict(imu_msg);对单次的IMU测量值做积分得到位移和姿态. //imu_msg [采样时间内单次IMU测量值]

2.1 un_acc_0, un_acc_1,un_gyr根据加速度计的观测值可得.然后采用中值//tmp_Q是local-->world

2.2 tem_P和tem_V即下面的公式所得:

(这里的计算得到的pvq是估计值，注意是没有观测噪声和偏置的结果.)///作用是与下面预积分计算得到的pvq（考虑了观测噪声和偏置）做差得到残差。这句话在下面做回应.

raw_image_callback:

  只有进行闭环检测的时候才用到图像

  img_msg [回调的图像feature_callback

feature_callback:

   关键点回调函数]
   feature_msg [订阅的关键点]

process函数:

1. getMeasurements()

1.1 如果最新的IMU的数据时间戳小于最旧特征点的时间戳，则等待IMU刷新；如果最旧的IMU数据的时间戳大于最旧特征时间戳，则弹出旧图像

1.2 这里IMU和Feature做了简单的对齐，确保IMU的时间戳是小于图像的 ; 在IMU buff中的时间戳小于特征点的都和该帧特征联合存入

2. 遍历获取的Features和IMU测量数据,

     send_imu  (imu_msg [IMU数据])

2.1 processIMU(dt ,linear_acceleration ,angular_velocity)

2.1.1 当滑窗不满的时候，把当前测量值加入到滑窗指定位置

2.1.2 残差和雅可比矩阵、协方差矩阵保存在pre_integrations中 (这里进入了头文件)

                 propagate (_dt[时间间隔]; _acc_1 [加速度计数据]; _gyr_1 [陀螺仪数据])

里面主要为 midPointIntegration 函数即: 中值法进行预积分

最终的矩阵形式如下:具体推导见IMU预积分及残差雅克比计算

得到状态变化量result_delta_q，result_delta_p，result_delta_v，result_linearized_ba，result_linearized_bg和得到跟新协方差矩阵和雅可比矩阵

2.1.3 提供imu计算的当前旋转，位置，速度，作为优化的初值

3. (跳回process函数了)

(这一块不理解)

4. processImage(image [特征点] , header [帧头]) 特征点处理程序

4.1 addFeatureCheckParallax

        向Featuresmanger中添加Features并确定共视关系及视差角的大小

4.1.1 FeaturePerFrame 特征点在某个图像帧下的坐标（归一化平面坐标）

4.1.2 遍历特征点,看该特征点是否在特征点的列表中,如果没在,则将<FeatureID,Start_frame>存入到Feature列表中

如果该Feature之前被观测到过,统计数目

4.1.3 计算共视关系， parallax_num为满足要求的Feature的个数

4.1.3.1 至少有两帧观测到该特征点(起始帧要小于倒数第二帧，终止帧要大于倒数第二帧，这样至少有三帧观测到该Feature(包括当前帧))

4.1.3.2 判断观测到该特征点的frame中倒数第二帧和倒数第三帧的共视关系实际是求取该特征点在两帧的归一化平面上的坐标点的距离ans

4.1.3.3 得到视差总和并统计个数

4.1.4 得到平均视差,平均视差视差要大于某个阈值, MIN_PARALLAX=10，大约是10个像素点.

这样的帧即是关键帧,边缘化最老的帧.否则不是关键帧,边缘化第二帧

4.2 将图像数据和时间存到图像帧类中：首先将数据和时间保存到图像帧的对象imageframe中（ImageFrame对象中包含特征点，时间，位姿R，t，预积分对象pre_integration，是否是关键帧）,同时将临时的预积分值保存到此对象中（这里的临时预积分初值就是在前面IMU预积分的时候计算的,即2.1.2），然后将图像帧的对象imageframe保存到all_image_frame对象中（imageframe的容器），更新临时预积分初始值

4.3 标定相机和IMU的外参数：接着如果没有外部参数就标定外部参数，参数传递有的话就跳过这一步（默认有，如果是自己的设备，可以设置为2对外参进行在线标定）

主要是CalibrationExRotation函数,

        corres[一组匹配的特征点] ,delta_q_imu即k ==> k+1,calib_ric_result [Camera与IMU的外参之旋转量]

4.3.1 滑窗内帧数加1

4.3.2 solveRelativeR,这是sfm中根据一组匹配点,求解出两帧的之间的变换矩阵添加到Rc中

4.3.3 将IMU预积分出的旋转四元数转换成矩阵形式

4.3.4 求取估计出的相机与IMU之间旋转的残差

4.3.5 计算外点剔除的权重

4.3.6 求取系数矩阵即:相机对极关系得到的旋转q的左乘和IMU预积分得到的旋转q的右乘

4.3.7 通过SVD分解，求取相机与IMU的相对旋转

            解为系数矩阵A的右奇异向量V中最小奇异值对应的特征向量

4.3.8 判断对于相机与IMU的相对旋转是否满足终止条件

         //！1.用来求解相对旋转的IMU-Cmaera的测量对数是否大于滑窗大小。
         //！2.A矩阵第二小的奇异值是否大于某个阈值，使A得零空间的秩为1

这部分在推导在初始化中相对旋转部分

4.4 判断是否需要初始化.

4.4.1 滑窗中的Keyframe达到指定大小的时候，才开始优化

4.4.2 初始化initialStructure()

4.4.2.1 保证IMU充分运动，通过线加速度判断，一开始求all_image_frame中所有帧的平均加速度, 并计算加速度的方差,q加速度的标准差（离散程度）判断保证IMU充分运动，加速度标准差大于0.25则代表imu充分激励，足够初始化.

纯视觉的初始化 sfm { 4.4.2.2 遍历滑窗内所有的Features

4.4.2.3 relativePose 在滑窗中寻找与最新的关键帧共视关系较强的关键帧

4.4.2.3.1 getCorresponding 选取两帧之间共有的Features

4.4.2.3.2 共视的Features应该大于20

4.4.2.3.3 求取所有匹配的特征点的平均视差,

4.4.2.3.4 视差大于一定阈值，并且能够有效地求解出变换矩阵 solveRelativeRT 中利用cv::findFundamentalMat计算基础矩阵用5点法来得到变换矩阵

这里找到的关键帧设置为l帧

4.4.2.4 sfm.construct

4.4.2.4.1 将第l帧当做原点，建立坐标系,然后将Frame转换到同一坐标系下。

4.4.2.4.2 获得最后一帧到l帧的变换关系

4.4.2.4.3 获得世界坐标系到l帧的变换关系

4.4.2.4.4 求取世界坐标系到最后一帧到的变换关系

4.4.2.4.5 先第l帧和最后一帧做三角化,根据三角化的点PnP求解第l+1的位姿,再l+1和最后一帧三角化,再PnP求l+2位姿,依次求到倒数第二帧的位姿

4.4.2.4.6 第l+1帧~倒数第二帧的所有帧与第l帧三角化剩下的特征点

4.4.2.4.7 从l帧开始往第一帧，逐渐帧pnp，再与第l帧进行三角定位得到更多的三维点

4.4.2.4.8 剩下的那些还没有被三角定位的特征点，通过它被观察到的第一帧和最后一帧进行三角定位。

4.4.2.4.9 BA优化，使用的是ceres优化位姿和特征点，residuals用的特征点到每一帧重投影误差, 将求解结果写出到q,T和sfm_tracked_points

4.4.2.5 solve pnp for all frame 用pnp算出在这段时间区域内的所有图像的位姿。每个图像的计算都用下一个关键帧的位姿来当pnp的初值。

4.4.2.6 if (visualInitialAlign()) 进入视觉与IMU的对齐,联合初始化

4.4.2.6.1 VisualIMUAlignment

4.4.2.6.1.1 solveGyroscopeBias(all_image_frame, Bgs); 修正陀螺仪的bias

4.4.2.6.1.1.1 $hat{gamma} _{b_{k+1}}^{b_{k}}*begin{bmatrix} 1 & \ frac{1}{2}J_{b_{w} }^{gamma}delta b_{w} & end{bmatrix}=q_{b_{k+1}}^{c_{0}}cdot {q_{b_{k}}^{c_{0}}}^{-1}$ 公式的右侧

4.4.2.6.1.2 将残差移动到右侧即: $begin{bmatrix} 1 & \ frac{1}{2}J_{b_{w} }^{gamma}delta b_{w} & end{bmatrix}=q_{b_{k+1}}^{c_{0}}cdot {q_{b_{k}}^{c_{0}}}^{-1}*hat{gamma} _{b_{k+1}}^{b_{k}}$

4.4.2.6.1.3 JTJ·bw = 2JT(r_bkbk+1^' × q_cb0' × q_cb1).vec

4.4.2.6.1.4 sum(A)bw = sum(b)

4.4.2.6.1.5 使用Cholesky分解，求解

4.4.2.6.1.6 将求得的陀螺仪偏置δbw是变化量, 所以将每帧的Bgs[i]都加上该delta量进行更新，最后在利用更新后的gyro bias重新repropagate.

4.4.2.6..1.2 LinearAlignment(all_image_frame, g, x) // 初始化速度、重力向量及尺度因子

即求解: