多传感器融合SLAM22 --- VINS-FUSION单目 + IMU + 在线估计IMU相机外参IMU偏移 SFM部分---GlobalSFM::construct代码解析

目录

1 这个函数的作用是什么、传入传出参数是什么

2 代码解析 

2.1 初始化向量

2.2 处理第l帧到第frame_num - 1帧之间的所有帧

2.2.1 GlobalSFM::solveFrameByPnP函数解析

2.2.2 GlobalSFM::triangulateTwoFrames函数解析

2.2.3 GlobalSFM::triangulatePoint函数解析

2.3 三角化第l帧到第frame_num - 1帧的所有帧 

2.4 第l帧开始，向前遍历到第0帧。对于每一帧，它首先解决了PnP问题，然后执行三角测量来估计特征点的三维位置。

2.5 对所有特征点进行三角化 

2.6 建立ceres求解问题 

2.6.1 添加旋转平移 并固定帧

2.6.2 添加优化因子 

2.7 将优化结果返回 

---

1 这个函数的作用是什么、传入传出参数是什么

        计算窗口内的相机的位姿，保存在Quaterniond* q, Vector3d* T内，大小为frame_num +1。

        frame_count + 1：frame_count保存是第几帧

        relative_R：帧到第WINDOW_SIZE的帧的旋转

        relative_T：帧到第WINDOW_SIZE的帧的平移

        Q：输入参数 Quaterniond Q[frame_count + 1];

        T ：输入参数Vector3d T[frame_count + 1];

        sfm_tracked_points ： 输入参数 map<int, Vector3d> sfm_tracked_points

        sfm_f：如下数