VINS-FUSION代码超详细注释（VIO部分）/VIO入门(3)

0 前情回顾

VINS-FUSION代码超详细注释（VIO部分）/VIO入门(1)讲到了主程序rosNodeTest.cpp。在程序最后，会进入sync_process线程进行处理。本篇博客接着进行讲解。
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(2)中,讲了sync_process,以及其中的trackImage和processMeasurements,包括processMeasurements中对IMU数据的处理部分.
本文开始说明图片处理的部分

本次工作

我首先一步步的把代码全部注释了，十分的详细，对于C++和OpenCV的一些操作也进行了详细的注释，对于刚入门的同学应该还是有帮助的。之后我将代码开源，并写了相应的博客进行讲解。

开源程序:

https://github.com/kuankuan-yue/VINS-FUSION-leanrning.git

相应博客：

VINS-FUSION代码超详细注释（VIO部分）/VIO入门(1)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(2)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(3)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(4)

3 processImage

函数入口processMeasurements中的processImage(feature.second, feature.first);输入的之后关键帧和时间

3.1 判断关键帧addFeatureCheckParallax

/* addFeatureCheckParallax
对当前帧与之前帧进行视差比较，如果是当前帧变化很小，就会删去倒数第二帧，如果变化很大，就删去最旧的帧。并把这一帧作为新的关键帧
这样也就保证了划窗内优化的,除了最后一帧可能不是关键帧外,其余的都是关键帧
VINS里为了控制优化计算量，在实时情况下，只对当前帧之前某一部分帧进行优化，而不是全部历史帧。局部优化帧的数量就是窗口大小。
为了维持窗口大小，需要去除旧的帧添加新的帧，也就是边缘化 Marginalization。到底是删去最旧的帧（MARGIN_OLD）还是删去刚
刚进来窗口倒数第二帧(MARGIN_SECOND_NEW)
如果大于最小像素,则返回true */
bool FeatureManager::addFeatureCheckParallax

判断之后,确定marg掉那个帧

// 检测关键帧
if (f_manager.addFeatureCheckParallax(frame_count, image, td))
{
    marginalization_flag = MARGIN_OLD;//新一阵将被作为关键帧!
    //printf("keyframe\n");
}
else
{
    marginalization_flag = MARGIN_SECOND_NEW;
    //printf("non-keyframe\n");
}

3.2 估计相机和IMU的外参

// 估计一个外部参,并把ESTIMATE_EXTRINSIC置1,输出ric和RIC
if(ESTIMATE_EXTRINSIC == 2)

首先找到对应的图像

vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            // 这个里边放的是新图像和上一帧

使用CalibrationExRotation计算参数

/* CalibrationExRotation
当外参完全不知道的时候，可以在线对其进行初步估计,然后在后续优化时，会在optimize函数中再次优化。
输入是新图像和上一阵图像的位姿 和二者之间的imu预积分值,输出旋转矩阵
对应VIO课程第七讲中对外参矩阵的求解 */
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)

估计外部参数使用理论指导的,在VIO课程的第七讲中有介绍.

3.3 如果没有初始化,那么就先进行初始化

如果没有的话,进行初始化部分.
【泡泡读者来稿】VINS 论文推导及代码解析（二）
有对初始化的详细理论介绍

3.3.1 单目+IMU的初始化

if (!STEREO && USE_IMU)

//有外参且当前帧时间戳大于初始化时间戳0.1秒，就进行初始化操作
if(ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)//initial_timestamp设为了0
{
    result = initialStructure(); //视觉惯性联合初始化
    initial_timestamp = header;   //更新初始化时间戳
}

其中的initialStructure还含有很多函数.

// 视觉和惯性的对其,对应https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw中的visualInitialAlign
/* visualInitialAlign
很具VIO课程第七讲:一共分为5步:
1估计旋转外参. 2估计陀螺仪bias 3估计中立方向,速度.尺度初始值 4对重力加速度进一步优化 5将轨迹对其到世界座标系 */
bool Estimator::visualInitialAlign()


// 视觉IMu的对其
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)


// 更新得到新的陀螺仪漂移Bgs
// 对应视觉IMU对其的第二部分
// 对应https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw中的公式31-34
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)


// 初始化速度、重力和尺度因子
// 对应 https://mp.weixin.qq.com/s/9twYJMOE8oydAzqND0UmFw 中的公式34-36
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)

【泡泡读者来稿】VINS 论文推导及代码解析（二）中都有详细的介绍.可以参考.
成功初始化之后
其中的optimization() updateLatestStates() slideWindow()在下一篇博客进行讲解

if(result)//如果初始化成功
{
    optimization();//先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿
    updateLatestStates();
    solver_flag = NON_LINEAR;
    slideWindow();//滑动窗口
    ROS_INFO("Initialization finish!");
}
else//滑掉这一窗
    slideWindow();

3.3.2 双目+IMU初始化

// stereo + IMU initilization
if(STEREO && USE_IMU)

求解深度

// 双目三角化
// 结果放入了feature的estimated_depth中
void FeatureManager::triangulate(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])


// 利用svd方法对双目进行三角化
void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
                        Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)

有了深度之后就可以进行PnP求解

// 有了深度，当下一帧照片来到以后就可以利用pnp求解位姿了
void FeatureManager::initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])

之后求解陀螺仪的偏执,并对IMU预积分值进行重新传播

solveGyroscopeBias(all_image_frame, Bgs);

// 对之前预积分得到的结果进行更新。
// 预积分的好处查看就在于你得到新的Bgs，不需要又重新再积分一遍，可以通过Bgs对位姿，速度的一阶导数，进行线性近似，得到新的Bgs求解出MU的最终结果。
for (int i = 0; i <= WINDOW_SIZE; i++)
{
    pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}

在至今进行优化

optimization();
updateLatestStates();
solver_flag = NON_LINEAR;
slideWindow();

其中的optimization() updateLatestStates() slideWindow()在下一篇博客进行讲解

3.3.3双目初始化

// stereo only initilization
if(STEREO && !USE_IMU)
{
    f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
    f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
    optimization();

    if(frame_count == WINDOW_SIZE)
    {
        optimization();
        updateLatestStates();
        solver_flag = NON_LINEAR;
        slideWindow();
        ROS_INFO("Initialization finish!");
    }
}

3.4 如果已经进行了初始化

进行优化,移除特点等操作.
在下一个博客进行详细讲解.