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-预积分R, -
然后在滑窗所有帧的约束中优化求解Bg,利用新的Bg重新求预积分。
然后后端优化:optimization
然后滑窗:slideWindow
updateLatestStates()
就是把当前状态放到前次状态,为下一次做准备
如果已经初始化
就不了,而是用视觉特征点、imu预积分联合状态估计。
首先三角化特征点空间位置:
f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
然后
optimization()
同上
slideWindow()
同上