初始化部分,对应解析中14页
初始化采用了视觉和IMU的松耦合方案,
首先用SfM求解滑窗内所有帧的位姿,和所有路标点的3D位置;
然后跟IMU预积分的值对齐,求解重力方向、尺度因子、陀螺仪bias及每一帧对应的速度。
先看SfM部分,发现initial_sfm.h
文件中有一个GlobalSFM
类,其下有一个成员函数construct
,就从这个函数看起。
其中,int frame_num
是GlobalSFM
类的唯一的数据成员。且在进入construct
第一句被赋值feature_num = sfm_f.size();
而函数参数sfm_f
本身是有值的?还是说仅仅用来带出结果的?(因为是引用);
bool construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
const Matrix3d relative_R, const Vector3d relative_T,
vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points);