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);
- 对于还没有深度的特征点进行三角化(双目立体视觉)，详见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() 同上
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
