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 如果已經進行了初始化

進行優化,移除特點等操作.
在下一個博客進行詳細講解.