vins博客的一部分6

processImage()

输入是本帧的特征点[id (cam_id, xyz_uv_vxvy)],包含了检测关键帧,估计外部参数,初始化,状态估计,划窗等等

检测关键帧,选择margin帧: addFeatureCheckParallax()

• 检测和上一帧公共特征点的个数,少 则新帧是特征帧，margin掉最老的关键帧
• 检测和上一帧公共特征点的视差,平均视差大的,也认为新帧是特征帧

if (f_manager.addFeatureCheckParallax(frame_count, image, td))
    marginalization_flag = MARGIN_OLD;//新一帧将被作为关键帧(标志位MARGIN_OLD=1)

有几个类：
FeatureManager：特征点管理类，主要包含list feature；
FeaturePerId：第id个特征点的信息：出现次数、逆深度、vector feature_per_frame；
vector feature_per_frame：该特征点在各个图像中的信息：位置、速度等。

估计外部参数

通过标志位ESTIMATE_EXTRINSIC(外部_估计)来判断是否标定过外参,当外参完全不知道的时候，可以在线对其进行初步估计,然后在后续优化时，会在optimize函数中再次优化。
通过initial_ex_rotation.CalibrationExRotation( )函数进行估计外参, 输入是两帧之间的多个匹配点对坐标的vector数组, 二者之间的旋转的预积分值delta_q, 输出旋转矩阵Rbc

联合初始化(如果还没有初始化)

用视觉PnP求位姿，同时求imu的Bg。

• 双目PnP，以imu积分值作为初值，优化求解出得到本帧的位姿Rs Ps，详见initFramePoseByPnP,

f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);深度已知,用PnP
//Ps, Rs, tic(平移外参), ric(旋转外参)是输出的结果

• 对于还没有深度的特征点进行三角化(双目立体视觉)，详见triangulate:

f_manager.triangulate(frame_count, Ps, Rs, tic, ric);

• 当第一次滑窗填满了后，即if (frame_count == WINDOW_SIZE), 然后就将窗口内各帧图像PnP求得的Rs Ps放到ImageFrame::R T中。

• 然后用solveGyroscopeBias()求解角速度的偏置Bg（Ba在初始化时和重力对齐了，就没再求解）,利用目标函数: 视觉计算出的相邻帧间的旋转 = IMU预积分的旋转值, 即: 偏置Bg = 视觉Ｒ-预积分R，

• 然后在滑窗所有帧的约束中优化求解Bg，利用新的Bg重新求预积分。

然后后端优化：optimization

然后滑窗：slideWindow

updateLatestStates()就是把当前状态放到前次状态,为下一次做准备

如果已经初始化

就不了,而是用视觉特征点、imu预积分联合状态估计。

首先三角化特征点空间位置:

f_manager.triangulate(frame_count, Ps, Rs, tic, ric);

然后

optimization() 同上

slideWindow() 同上