VIN3.系統初始化

引言

搬運工 + 自己閱讀代碼理解。

參考說明

pipeline如圖：

本節則是閱讀總結Initialization部分。estimator.cpp-->processImage().

在提取的圖像的 Features 和做完 IMU 的預積分之後,進入了系統的初始化環節,首先,利用 SFM 進行純視覺估計滑窗內所有幀的位姿及 3D 點逆深度,最後與 IMU 預積分進行對齊求解初始化參數。主要的目的有以下兩個:

系統使用單目相機,如果沒有一個良好的尺度估計,就無法對兩個傳感器做進一步的融合,這個時候需要恢復出尺度;
要對 IMU 進行初始化,IMU 會受到 bias 的影響,所以要得到 IMU 的 bias。

所以需要從初始化中恢復出尺度、重力、速度以及 IMU 的 bias,因爲視覺 (SFM) 在初始化的過程中有着較好的表現,所以在初始化的過程中主要以 SFM 爲主,然後將 IMU 的預積分結果與其對齊，即可得到較好的初始化結果。初始化採用視覺和 IMU 的松耦合方案,首先用 SFM 求解滑窗內所有幀的位姿,和所有路標點的 3D 位置,然後跟 IMU 預積分的值對齊,求解重力方向、尺度因子、陀螺儀 bias 及每一幀對應的速度。

1.初始化流程

初始化的流程圖如下所示:

首先,對代碼中幾個易混變量進行說明:

傳到 processImage 的 image 表示當前幀跟蹤到上一幀中的特徵點集合,也就是當前幀觀測到的所有的路標點(不包括在當前幀新提取的點),如VIN1.視覺前端表中 track_cnt≥2 的 feature,image 類型爲:

FeatureManager 中幾個變量:

VINS裏面分四步進行estimator.cpp-->processImage():

1.第一個就是旋轉外參校準;
2.第二個就是找到某幀作爲系統初始化原點，計算3D地圖點;(下面的相機初始化)
3.第三就是將相機座標系轉到IMU座標系中;
4.第四就是相機與IMU對齊，包括IMU零偏初始化，速度，重力向量，尺度初始化.

相機初始化estimator.cpp-->processImage()-->initialStructure()函數：

先判斷IMU是否有足夠的variance, 求all_image_frame中所有幀的平均加速度, 並計算加速度的方差, 判斷方差是否足夠大

調用relativePose判斷sliding window是否某一幀l與當前幀視差足夠大, 如果存在的話, 則根據特徵匹配用五點法計算l和當前幀的relative R,t, 用來做後面的SFM

調用GlobalSFM.construct進行SFM, 估計slidingwindow中的camera pose和features的3D position.

然後對all_image_frame中沒有參與SFM的幀的位姿進行了PnP求解.

調用visualInitialAlign()進行視覺和IMU的校準.

簡潔：

求取本質矩陣求解位姿(relativePose)

三角化特徵點(sfm.construct)

PnP 求解位姿(cv::solvePnP)

轉換到 IMU 座標系下

$c_0$ 座標系作爲參考系

不斷重複直到恢復出滑窗內的 Features 和相機位姿

2.CalibrationExRotation

estimator.cpp-->processImage()-->CalibrationExRotation()；轉換爲數學模型實際就是求解超定方程組，可以利用最小二乘也可以使用奇異值分解，由於一般不滿秩故採用奇異值分解求解。
　　相對旋轉是相機與 IMU 之間外參的一部分。相機與 IMU 之間的旋轉標定非常重要,偏差 $1～2^{\circ}$ ，系統的精度就會變得很低。通過視覺 SFM 獲取了相鄰兩關鍵幀之間相機的相對旋轉 $R_{c_{k+1}}^{c_{k}}$ ,IMU 經過預積分得到的旋轉矩陣爲 $R_{b_{k+1}}^{b_{k}}$ 。相機與 IMU 之間的相對旋轉爲 $R_{c}^{b}$ ,則對於任一幀滿足：
$R_{b_{k+1}}^{b_{k}} R_{c}^{b}=R_{c}^{b} R_{c_{k+1}}^{c_{k}}\tag{1}$ 將旋轉矩陣寫爲四元數,則上式可以寫爲 $q_{b_{k+1}}^{b_{k}} \otimes q_{c}^{b}=q_{c}^{b} \otimes q_{c_{k+1}}^{c_{k}}\tag{2}$ 將其寫爲左乘和右乘的形式 $\left(\left[q_{b_{k+1}}^{b_{k}}\right]_{L}-\left[q_{c_{k+1}}^{c_{k}}\right]_{R}\right) q_{c}^{b}=Q_{k+1}^{k} q_{c}^{b}=0\tag{3}$ $[q]_{L}$ 、 $[q]_{R}$ 分別表示四元數左乘矩陣和四元數右乘矩陣,其定義爲(四元數實部在後) $L=\left[\begin{array}{cc}{q_{w} I_{3}+\left[q_{x y z}\right]_{ \times}} & {q_{x y z}} \\ {-q_{x y z}} & {q_{w}}\end{array}\right]$ $[q]_{R}=\left[\begin{array}{cc}{q_{w} I_{3}-\left[q_{x y z}\right]_{ \times}} & {q_{x y z}} \\ {-q_{x y z}} & {q_{w}}\end{array}\right]\tag{4}$ 那麼對於 n 對測量值,則有 $\left[\begin{array}{c}{w_{1}^{0} Q_{1}^{0}} \\ {w_{2}^{1} Q_{2}^{1}} \\ {\vdots} \\ {w_{N}^{N-1} Q_{N}^{N-1}}\end{array}\right] q_{c}^{b}=Q_{N} q_{c}^{b}=0\tag{5}$ 其中 $w_{N}^{N-1}$ 爲外點剔除權重,其與相對旋轉求得的角度殘差有關, $N$ 爲計算相對旋轉需要的測量對數,其由最終的終止條件決定。角度殘差可以寫爲 $\theta_{k+1}^{k}=\arccos \left(\frac{\operatorname{tr}\left(\hat{R}_{c}^{b^{-1}} R_{b_{k+1}}^{b_{k}^{-1}} \hat{R}_{c}^{b} R_{c_{k+1}}^{c_{k}}\right)-1}{2}\right)\tag{6}$
從而權重爲 $w_{k+1}^{k}=\left\{\begin{array}{cc}{1,} & {\theta_{k+1}^{k}<\text {threshold}（一般爲5~度） } \\ {\frac{t h r e s h o l d}{\theta_{k+1}^{k}},} & {\text {otherwise}}\end{array}\right.\tag{7}$ 至此,就可以通過求解方程 $Q_{N} q_{c}^{b}=0$ 得到相對旋轉，解爲 $Q_{N}$ 的左奇異向量中最小的奇異值對應的特徵向量(是最小二乘法的最優解，之前證明過)
　　但是,在這裏還要注意求解的終止條件 (校準完成的終止條件) 。在足夠多的旋轉運動中,我們可以很好的估計出相對旋轉 $R_{c}^{b}$ ,這時 $Q_{N}$ 對應一個準確解,且其零空間的秩爲 1。但是在校準的過程中,某些軸向上可能存在退化運動 (如勻速運動),這時 $Q_{N}$ 的零空間的秩會大於 1。判斷條件就是 $Q_{N}$ 的第二小的奇異值是否大於某個閾值，若大於則其零空間的秩爲1，反之秩大於1，相對旋轉 $R_{c}^{b}$ 的精度不夠，校準不成功。

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    //! Step1：滑窗內的幀數加1
    frame_count++;

    //! Step2:計算兩幀之間的旋轉矩陣
    Rc.push_back(solveRelativeR(corres));

    //! IMU 預積分得到的旋轉
    Rimu.push_back(delta_q_imu.toRotationMatrix());
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);

    Eigen::MatrixXd A(frame_count * 4, 4);
    A.setZero();
    int sum_ok = 0;
    for (int i = 1; i <= frame_count; i++)
    {
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);

        //！Step3:求取估計出的相機與IMU之間旋轉的殘差
        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        //ROS_DEBUG("%d %f", i, angular_distance);

        //! Step4:計算外點剔除的權重
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;

        //! Step5：求取係數矩陣，公式(3)的係數矩陣Q
        //! 得到相機對極關係得到的旋轉q的左乘
        double w = Quaterniond(Rc[i]).w();
        Vector3d q = Quaterniond(Rc[i]).vec();
        L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
        L.block<3, 1>(0, 3) = q;
        L.block<1, 3>(3, 0) = -q.transpose();
        L(3, 3) = w;

        //! 得到由IMU預積分得到的旋轉q的右乘
        Quaterniond R_ij(Rimu[i]);
        w = R_ij.w();
        q = R_ij.vec();
        R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
        R.block<3, 1>(0, 3) = q;
        R.block<1, 3>(3, 0) = -q.transpose();
        R(3, 3) = w;
		//拼接大矩陣Q，公式(5)的係數矩陣Q
        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }

    //！Step6：通過SVD分解，求取相機與IMU的相對旋轉
    //！解爲係數矩陣A的右奇異向量V中最小奇異值對應的特徵向量
    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    Matrix<double, 4, 1> x = svd.matrixV().col(3);
    Quaterniond estimated_R(x);
    ric = estimated_R.toRotationMatrix().inverse();
    //cout << svd.singularValues().transpose() << endl;
    //cout << ric << endl;

    //！Step7：判斷對於相機與IMU的相對旋轉是否滿足終止條件
    //！1.用來求解相對旋轉的IMU-Cmaera的測量對數是否大於滑窗大小。
    //！2.A矩陣第二小的奇異值是否大於某個閾值，使A得零空間的秩爲1
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}

接下來介紹estimator.cpp-->processImage()-->initialStructure()函數流程：

3.檢測 IMU 可觀性

estimator.cpp-->processImage()-->initialStructure()：通過計算線加速度的標準差，判斷IMU是否有充分運動激勵，以進行初始化注意這裏並沒有算上all_image_frame的第一幀，所以求均值和標準差的時候要減一。參考文章爲什麼標準差可以代表可觀性？

// 計 算 均 值
map<double, ImageFrame>::iterator frame_it;
Vector3d sum_g;
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
    double dt = frame_it->second.pre_integration->sum_dt;
    Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
    sum_g += tmp_g;
}
// 計 算 方 差
Vector3d aver_g;
aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);
double var = 0;
for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++)
{
    double dt = frame_it->second.pre_integration->sum_dt;
    Vector3d tmp_g = frame_it->second.pre_integration->delta_v / dt;
    var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
    //cout << "frame g " << tmp_g.transpose() << endl;
}
// 計 算 標 準 差
var = sqrt(var / ((int)all_image_frame.size() - 1));
//ROS_WARN("IMU variation %f!", var);
if (var < 0.25)// ! 以 標 準 差 判 斷 可 觀 性
{
    // ROS_INFO("IMU excitation not enouth!");
    //return false;
}

4.relativePose

estimator.cpp-->processImage()-->initialStructure()-->relativePose();在滑窗內尋找與當前幀的匹配特徵點數較多的關鍵幀作爲參考幀, 並通過求本質矩陣（E）cv::findFundamentalMat 計算出當前幀到參考幀的 T;參考E2Rt.

/**
 * [Estimator::relativePose 在滑窗中尋找與最新的關鍵幀共視關係較強的關鍵幀]
 * @param  relative_R [description]
 * @param  relative_T [description]  window_size ==> i
 * @param  l          [description]
 * @return            [description]
 */
bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)

solve_Spts.cpp–>solveRelativeRT()

/**
 * [MotionEstimator::solveRelativeRT 利用5點法求解變換矩陣T]
 * @param  corres      [匹配點]
 * @param  Rotation    [旋轉量]
 * @param  Translation [平移量]
 * @return             [description]
 */
 bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)

這個函數調用opencv函數：cv::Mat E = cv::findFundamentalMat(*)求解出本質矩陣E,再利用E求解出T(R、t)參考E2Rt.VINs中代碼solve_Spts.cpp-->recoverPose如下：

    /**
     * 從本質矩陣中恢復出變換矩陣
     * 函數調用decomposeEssentialMat()利用SVD分解求解出四中可能然後再驗證出符合要求的唯一解，和十四講中的計算是一樣的！
     * @param E
     * @param _points1
     * @param _points2
     * @param _cameraMatrix
     * @param _R
     * @param _t
     * @param _mask 求解本質矩陣時得到的內外點的標誌位
     * @return
     */
     int recoverPose(*)

主要函數流程：relativePose()-->solveRelativeRT(求得E)-->recoverPose()[求得T]-->decomposeEssentialMat()[SVD分解]

5.GlobalSFM.construct

首先,將 feature 隊列,放到 vector sfm_f 中,其中 SFMFeature 中的observation 存放的是觀測到該路標點的 FrameId 及特徵點座標,如下所示:

GlobalSFM 的流程如下,其中,參考幀爲上一步選出來的最共視幀,最後得到的sfm_tracked_points 爲三角化出來的 3D 路標點。

幾個主要的函數：

1.estimator.cpp–>initialStructure()–>construct():

/**initial_sfm.cpp
 * [GlobalSFM::construct description]
 * @param  frame_num          [滑窗內Frame個數]
 * @param  q                  [旋轉量]
 * @param  T                  [平移量]
 * @param  l                  [共視幀的id]
 * @param  relative_R         now ===> l
 * @param  relative_T
 * @param  sfm_f              滑窗內所有的features
 * @param  sfm_tracked_points 三角化得到的points
 * @return
 * bool GlobalSFM::construct(*)
 */

2.estimator.cpp–>initialStructure()–>construct()–>solveFrameByPnP():

/**initial_sfm.cpp
 * [GlobalSFM::solveFrameByPnP PnP求解滑窗內關鍵幀的位姿]
 * @param  R_initial [description]  位姿，世界到相機座標系
 * @param  P_initial [description]
 * @param  i         [description]
 * @param  sfm_f     [description]
 * @return           [description]
 */
bool GlobalSFM::solveFrameByPnP()

PnP(Perspective-n-Point)是求解3D到2D點對運動的方法.已知 $n$ 個空間點 $[X, Y, Z, 1]$ (前一幀的相機座標系)以及他們的投影位置 $\left(u_{1}, v_{1}, 1\right)$ (歸一化平面座標系),使用直接線性變換(DLT)求解位姿。參考3D-2D:PnP_DLT.P3P。
$\left[\begin{array}{c}{u_{1}} \\ {v_{1}} \\ {1}\end{array}\right]=[R | t]_{3 * 4}\left[\begin{array}{l}{X} \\ {Y} \\ {Z} \\ {1}\end{array}\right]_{4 * 1}\tag{8}$

這個函數中主要的函數則是調用opencv的函數cv::solvePnP()調用OpenCV 的 PnP 求解，可選擇EPNP，DLS等方法.對滑窗內每一幀圖像,都跟上一步 SFM 得到的所有 3D 路標點,進行 cv::solvePnP 求解位姿。純視覺初始化時,採用第一幀 $c_{0}$ 作爲基準座標系,若要轉化爲從 body 座標系到 $c_{0}$ 座標系,可以進行如下變換:
$q_{b_{k}}^{c_{0}}=q_{c_{k}}^{c_{0}} \otimes\left(q_{c}^{b}\right)^{-1}\tag{9}$ $s \overline{p}_{b_{k}}^{c_{0}}=s \overline{p}_{c_{k}}^{c_{0}}-R_{b_{k}}^{c_{0}} p_{c}^{b}\tag{10}$
上式推導如下,這裏採用個人(崔神)較熟悉的寫法:
$T_{c_{0} \leftarrow b_{k}}=T_{c_{0} \leftarrow c_{k}} T_{b \leftarrow c}^{-1}$ $\Leftrightarrow T_{c_{0}-b_{k}} T_{b \leftarrow c}=T_{c_{0} \leftarrow c_{k}}$ $\Leftrightarrow R_{c_{0} \leftarrow b_{k}}\left(R_{b \leftarrow c} P_{c_{k}}+t_{b \leftarrow c}\right)+t_{c_{0}-b_{k}}=R_{c_{0} \leftarrow c_{k}} P_{c_{k}}+t_{c_{0} \leftarrow c_{k}}\tag{11}$ $\Longrightarrow\left\{\begin{array}{c}{R_{c_{0} \leftarrow b_{k}}=R_{c_{0} \leftarrow c_{k}} R_{b \leftarrow c}^{-1}} \\ {t_{c_{0} \leftarrow b_{k}}=t_{c_{0} \leftarrow c_{k}}-R_{c_{0} \leftarrow b_{k}} t_{b \leftarrow c}}\end{array}\right.$

3.estimator.cpp–>initialStructure()–>construct()–>triangulateTwoFrames():

/**initial_sfm.cpp
 * [GlobalSFM::triangulateTwoFrames 三角化恢復出兩幀共視的匹配點]
 * @param frame0 [特徵幀1]
 * @param Pose0  [位姿1]
 * @param frame1 [特徵幀2]
 * @param Pose1  [位姿2]
 * @param sfm_f  [特徵點集合]   求取的點在世界座標系
 */
void GlobalSFM::triangulateTwoFrames(*)

三角化這裏詳細推導過，求解過程和第二小節CalibrationExRotation相似，同樣是使用奇異值分解求解超定方程的最小二乘解，至於奇異值的最小特徵值對應的特徵向量爲什麼是最小二乘的最優解也推導過，不再做筆記。同時這個函數主要是調用triangulatepoint()函數進行三角化：

/**
 * [GlobalSFM::triangulatePoint 三角化恢復特徵點深度]
 * @param Pose0    [相機位姿1]
 * @param Pose1    [相機位姿2]
 * @param point0   [特徵點1]
 * @param point1   [特徵點2]
 * @param point_3d [恢復出的3D點]
 *
 *      求取的點在Pose0和Pose1的參考座標系下
 *
 * |uP2-P0|
 * |vP2-P1|
 * |uQ2-Q0|P=0  參考ORB-SLAM初始化部分，DLT求解
 * |vQ2-Q1|
 * 解爲矩陣P的最小奇異值對應的單位奇異矢量
 */
void GlobalSFM::triangulatePoint(*)

6.visualInitialAlign

這一節還有點蒙，懶得往上翻，再放一下這個圖：
IMU 預積分與視覺結構對齊示意圖：

SFM 與 IMU 預積分結果對齊主要解決 3 個問題:

陀螺儀偏置的校準;
初始化速度、重力向量和尺度因子;
重力向量的精調。

estimator.cpp-->processImage()-->initialStructure()-->visualInitialAlign();

/**vins_estimator/src/initial/initial_aligment.cpp
 * [VisualIMUAlignment 視覺與IMU對齊，主要解決三個問題：
 *                     1) 修正陀螺儀的bias
 *                     2）初始化速度、重力向量g和尺度因子(Metric scale)
 *                     3）改進重力向量的量值]
 * @param  all_image_frame [包含圖像特徵和IMU預積分的集合]
 * @param  Bgs             [陀螺儀bias]
 * @param  g               [重力向量]
 * @param  x               [尺度因子]
 * @return                 [description]
 */
bool VisualIMUAlignment(*)

6.1.陀螺儀 bias 校正

estimator.cpp-->processImage()-->initialStructure()-->visualInitialAlign()-->solveGyroscopeBias();
考慮滑動窗口中的連續兩個關鍵幀 $b_{k}$ 和 $b_{k+1}$ ,通過 SFM 可以獲取兩個關鍵幀時刻本體座標系相對於參考相機座標系的旋轉 $q_{b_{k}}^{c_{0}}$ 和 $q_{b_{k+1}}^{c_{0}}$ ,通過 IMU 預積分可以獲得兩個關鍵幀時刻本體座標系之間的相對旋轉約束 $\gamma_{b_{k+1}}^{b_{k}}$ ,校正陀螺儀偏置的目標函數爲:

$\min _{\delta b_{w}} \sum_{k \in B}\left\|q_{b_{k+1}}^{c_{0}-1} \otimes q_{b_{k}}^{c_{0}} \otimes \gamma_{b_{k+1}}^{b_{k}}\right\|^{2}\tag{12}$ 其中(一階Taylor) $\gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} J_{\delta b_{k_{k}}}^{\delta \theta_{b_{k+1}}^{b_k}} \delta b_{w_{k}}}\end{array}\right]\tag{13}$
B 表示滑動窗口中的所有關鍵幀,等式(13)爲 IMU 預積分中推導的, $\gamma_{b_{k+1}}^{b_{k}}$ 關於陀螺儀偏置誤差 $\delta b_{w}$ 的一階近似。

等式(12)中目標函數的最小值爲單位四元數,所以可將等式(12)進一步改寫爲(這裏的四元數採用實部在前,虛部在後的形式):
$q_{b_{k+1}}^{c_{0}-1} \otimes q_{b_{k}}^{c_{0}} \otimes \gamma_{b_{k+1}}^{b_{k}}=\left[\begin{array}{c}{1} \\ {0} \\ {0} \\ {0}\end{array}\right]$ $\hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} J_{\delta b_{w_{k}}}^{\delta \theta_{b_{k+1}}^{b_k}} \delta b_{w_{k}}}\end{array}\right]=q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}} \otimes\left[\begin{array}{c}{1} \\ {0} \\ {0} \\ {0}\end{array}\right]\tag{14}$ $\left[\begin{array}{c}{1} \\ {\frac{1}{2} J_{\delta b_{k+1}}^{\delta \theta_{b_{k+1}}^{b_k}} \delta b_{w_{k}}}\end{array}\right]=\hat{\gamma}_{b_{k+1}}^{b_{k}-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}} \otimes\left[\begin{array}{l}{1} \\ {0} \\ {0} \\ {0}\end{array}\right]$ 只考慮虛部,則有:

$J_{\delta b_{w_{k}}}^{\delta \theta_{b_{k+1}}^{b_k}}{\delta b_{w_k}}=2\left(\hat{\gamma}_{b_{k+1}}^{b_{k}-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\right)_{v e c}\tag{15}$ 將等式(15)中等號座標轉爲正定矩陣:
$[J_{\delta b_{w_{k}}}^{\delta \theta_{b_{k+1}}^{b_k}}]^TJ_{\delta b_{w_{k}}}^{\delta \theta_{b_{k+1}}^{b_k}}{\delta b_{w_k}}=2[J_{\delta b_{w_{k}}}^{\delta \theta_{b_{k+1}}^{b_k}}]^T\left(\hat{\gamma}_{b_{k+1}}^{b_{k}-1} \otimes q_{b_{k}}^{c_{0}-1} \otimes q_{b_{k+1}}^{c_{0}}\right)_{v e c}\tag{16}$
這樣就可以使用 cholesky 分解求解矩陣(16),獲取目標函數達到最小的解 $\delta b_{w}$ ,從而完成對陀螺儀偏置的校正。

/**
 * [solveGyroscopeBias description]
 * @param  all_image_frame [包含圖像特徵和IMU預積分的集合]
 * @param  Bgs             [陀螺儀bias]
 * 這部分可以參考文獻[1]中公式(15)部分(注：'表示求逆)
 *
 * Min sum||q_cb1' × q_cb0 × r_bkbk+1||      (1)
 * 
 * 公式(15)的最小值是1(q(1,(0,0,0)，所以將其前半部分移到右邊得
 *            |     1   |
 * r_bkbk+1^ ·| 1/2·J·bw|=q_cb0' q_cb1        (2)
 *
 *            |     1   |
 *            | 1/2·J·bw|=r_bkbk+1^' × q_cb0' × q_cb1        (3)
 * 
 * 只取四元數的虛部並求解，
 *            JTJ·bw = 2JT(r_bkbk+1^' × q_cb0' × q_cb1).vec  (4)
 * 即，A·bw=b，將多個幀綜合起來爲
 *            sum(A)bw = sum(b)                              (5)
 */

//gyro的標定思路是:對於相鄰的兩幀, vision有一個相對旋轉q, IMU預積分有一個相對旋轉γ, Align的目的就是尋找一個最好的δbw使得兩個相對旋轉儘可能接近
void solveGyroscopeBias(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs)

6.2.初始化速度、重力和尺度因子

estimator.cpp-->processImage()-->initialStructure()-->visualInitialAlign()-->LinearAlignment(×);
這一部分的內容對應於 VINS-Mono 代碼 initial_aligment.cpp 中的 LinearAlignment()函數。待優化的變量爲:
$\chi_{I}=\left[\begin{array}{c}{v_{b_{0}}^{b_{0}}} \\ {v_{b_{0}}^{b_{0}}} \\ {\vdots} \\ {v_{b_{0}}^{b_{0}}} \\ {g^{c_{0}}} \\ {s}\end{array}\right]\tag{17}$
將預積分項中的世界座標系 $w$ 換爲 $c_0$ 座標系:
$\alpha_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}}\left(p_{b_{k+1}}^{c_{0}}-p_{b_{k}}^{c_{0}}-v_{b_{k}}^{c_{0}} \Delta t_{k}+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}\right)\tag{18}$ $\beta_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}}\left(v_{b_{k+1}}^{c_{0}}-v_{b_{k}}^{c_{0}}+g^{c_{0}} \Delta t_{k}\right)\tag{19}$
$p_{b_{k+1}}^{c_{0}}$ 和 $p_{b_{k}}^{c_{0}}$ 可由視覺 SFM 獲得:
$p_{b_{k}}^{c_{0}}=s \overline{p}_{b_{k}}^{c_{0}}\tag{20}$ $p_{b_{k+1}}^{c_{0}}=s \overline{p}_{b_{k+1}}^{c_{0}}\tag{21}$ 將等式(20)(21)代入等式(18):
$\alpha_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{c}}\left(s\left(\overline{p}_{b_{k+1}}^{c_{0}}-\overline{p}_{b_{k}}^{c_{0}}\right)-v_{b_{k}}^{c_{0}} \Delta t_{k}+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}\right)\tag{22}$ 將等式(19)(22)中的速度都轉換到 $c_0$ 座標系下:
$\alpha_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}}\left(s\left(\overline{p}_{b_{k+1}}^{c_{0}}-\overline{p}_{b_{k}}^{c_{0}}\right)-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}} \Delta t_{k}+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}\right)\tag{23}$ $\beta_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}}\left(R_{b_{k+1}}^{c_{0}} v_{b_{k+1}}^{b_{k+1}}-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}}+g^{c_{0}} \Delta t_{k}\right)\tag{24}$ 想辦法將等式(23)(24)轉換成 $H x=b$ 的形式,這樣便於利用 cholesky 進行求解。將等式(10)帶入等式(23):

$\alpha_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}}\left(s\left(\overline{p}_{b_{k+1}}^{c_{0}}-\overline{p}_{b_{k}}^{c_{0}}\right)-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}} \Delta t_{k}+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}\right) \Rightarrow$

$\alpha_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}}\left(\left(s \overline{p}_{c_{k+1}}^{c_{0}}-R_{b_{k+1}}^{c_{0}} p_{c}^{b}\right)-\left(s \overline{p}_{c_{k}}^{c_{0}}-R_{b_{k}}^{c_{0}} p_{c}^{b}\right)-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}} \Delta t_{k}+\frac{1}{2} g^{c_{0}} \Delta t_{k}^{2}\right) \Rightarrow$

$\alpha_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}} s\left(\overline{p}_{c_{k+1}}^{c_{0}}-\overline{p}_{c_{k}}^{b_{0}}\right)-R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}+p_{c}^{b}-v_{b_{k}}^{b_{k}} \Delta t_{k}+\frac{1}{2} R_{c_{0}}^{b_{k}} g^{c_{0}} \Delta t_{k}^{2} \Rightarrow$
$\tag{25}$
$-v_{b_{k}}^{b_{k}} \Delta t_{k}+\frac{1}{2} R_{c_{0}}^{b_{k}} g^{c_{0}} \Delta t_{k}^{b_{k}}\left(\overline{p}_{c_{k+1}}^{c_{0}}-\overline{p}_{c_{k}}^{c_{0}}\right) s=\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b} \Rightarrow$

$\left[\begin{array}{cccc}{-I \Delta t_{k}} & {0} & {\frac{1}{2} R_{c_{0}}^{b_{k}} \Delta t_{k}^{2}} & {R_{c_{0}}^{b_{k}}\left(\overline{p}_{c_{k+1}}^{c_{0}}-\overline{p}_{c_{k}}^{c_{0}}\right)}\end{array}\right]\left[\begin{array}{c}{v_{b_{k}}^{b_{k}}} \\ {v_{b_{k+1}}^{b_{k+1}}} \\ {g^{c_{0}}} \\ {s}\end{array}\right]=\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b}$

同樣地也將等式(24)轉換爲矩陣形式:

$\beta_{b_{k+1}}^{b_{k}}=R_{c_{0}}^{b_{k}}\left(R_{b_{k+1}}^{c_{0}} v_{b_{k+1}}^{b_{k+1}}-R_{b_{k}}^{c_{0}} v_{b_{k}}^{b_{k}}+g^{c_{0}} \Delta t_{k}\right) \Rightarrow$

$-v_{b_{k}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} v_{b_{k+1}}^{b_{k+1}}+R_{c_{0}}^{b_{2}} g^{c_{0}} \Delta t_{k}=\beta_{b_{k+1}}^{b_{k}} \Rightarrow$
$\tag{26}$
$\left[\begin{array}{cccc}{-I } & {R_{c_0}^{b_k}R_{c_0}^{b_{k+1}} } & { R_{c_{0}}^{b_{k}} \Delta t_{k}} & {0}\end{array}\right]\left[\begin{array}{c}{v_{b_{k}}^{b_{k}}} \\ {v_{b_{k+1}}^{b_{k+1}}} \\ {g^{c_{0}}} \\ {s}\end{array}\right]=\beta_{b_{k+1}}^{b_{k}}$

聯立等式(25)(26):
$\left[\begin{array}{cccc}{-I \Delta t_{k}} & {0} & {\frac{1}{2} R_{c_{0}}^{b_{x}} \Delta t_{k}^{2}} &{R_{c_{0}}^{b_{k}}\left(\overline{p}_{c_{k+1}}^{c_{0}}-\overline{p}_{c_{k}}^{c_{0}}\right)} \\ {-I} & {R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}}} & {R_{c_{0}}^{b_{k}} \Delta t_{k}} &{0} \end{array}\right] \left[\begin{array}{c}{v_{b_{k}}^{b_{k}}} \\ {v_{b_{k+1}}^{b_{k+1}}} \\ {g^{c_{0}}} \\ {s}\end{array}\right]=\left[\begin{array}{c}{\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b}} \\ {\beta_{b_{k+1}}^{b_{k}}}\end{array}\right]\tag{27}$
理論上等式(27)中的等號應該成立,但是由於各種噪聲與數值運算的誤差,等號並不嚴格成立,將理論真值 $\alpha_{b_{k+1}}^{b_{k}}, \beta_{b_{k+1}}^{b_{k}}$ 替換爲通過 IMU 測量值計算出的標稱值 $\hat{\alpha}_{b_{k+1}}^{b_{k}}, \hat{\beta}_{b_{k-1}}^{b_{k}}$ 得到約等式:

$\left[\begin{array}{cccc}{-I \Delta t_{k}} & {0} & {\frac{1}{2} R_{c_{0}}^{b_{x}} \Delta t_{k}^{2}} &{R_{c_{0}}^{b_{k}}\left(\overline{p}_{c_{k+1}}^{c_{0}}-\overline{p}_{c_{k}}^{c_{0}}\right)} \\ {-I} & {R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}}} & {R_{c_{0}}^{b_{k}} \Delta t_{k}} &{0} \end{array}\right] \left[\begin{array}{c}{v_{b_{k}}^{b_{k}}} \\ {v_{b_{k+1}}^{b_{k+1}}} \\ {g^{c_{0}}} \\ {s}\end{array}\right] \approx \left[\begin{array}{c}{\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b}} \\ {\beta_{b_{k+1}}^{b_{k}}}\end{array}\right]\tag{28}$

令：
$H_{b_{k+1}}^{b_{k}}=\left[\begin{array}{cccc}{-I \Delta t_{k}} & {0} & {\frac{1}{2} R_{c_{0}}^{b_{x}} \Delta t_{k}^{2}} &{R_{c_{0}}^{b_{k}}\left(\overline{p}_{c_{k+1}}^{c_{0}}-\overline{p}_{c_{k}}^{c_{0}}\right)} \\ {-I} & {R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}}} & {R_{c_{0}}^{b_{k}} \Delta t_{k}} &{0} \end{array}\right]\tag{29}$

$\chi_{b_{k+1}}^{b_{k}}=\left[\begin{array}{c}{v_{b_{k}}^{b_{k}}} \\ {v_{b_{k}}^{b_{k}}} \\ {v_{b_{k+1}}^{b_{k+1}}} \\ {g^{c_{0}}} \\ {s}\end{array}\right]\tag{30}$

$\hat{z}_{b_{k+1}}^{b_{k}}=c\left[\begin{array}{c}{\alpha_{b_{k+1}}^{b_{k}}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-p_{c}^{b}} \\ {\beta_{b_{k+1}}^{b_{k}}}\end{array}\right]\tag{31}$

約等式(28)可簡寫爲:
$H_{b_{k+1}}^{b_{k}} \chi_{b_{k+1}}^{b_{k}} \approx \hat{z}_{b_{k+1}}^{b_{k}}\tag{32}$
通過解線性最小二乘問題:
$\min _{\chi_{b_{k+1}}^{b_{k}}} \sum_{k \in B}\left\|\hat{z}_{b_{k+1}}^{b_{k}}-H_{b_{k+1}}^{b_{k}} \chi_{b_{k+1}}^{b_{k}}\right\|^{2}\tag{33}$
即可優化 $\chi_{I}$ ,即滑動窗口中所有關鍵幀的速度、重力向量在 $c_0$ 座標系中的分量列陣和單目尺度因子 $s$ 。

/**
 * [LinearAlignment 初始化速度、重力向量g和尺度因子(Metric scale)]
 * @param  all_image_frame [包含圖像特徵和IMU預積分的集合]
 * @param  g               [重力向量]
 * @param  x               [description]
 * @return                 [description]
 * 參考
 */
bool LinearAlignment(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)

6.3.修正重力矢量

對應代碼中 vins_estimator/src/initial/initial_aligment.cpp-->RefineGravity()函數.

假設重力的模值固定,且其數值爲已知量,這樣重力向量的自由度就由 3 變成了 2。因此將重力向量重新參數化:重力矢量的模長固定(9.8),其爲 2 個自由度,在切空間上對其參數化:
$\begin{aligned} \hat{g} &=\|g\| \overline{\hat{g}}+w_{1} b_{1}+w_{2} b_{2} \\ &=\|g\| \overline{\hat{g}}+\left[\begin{array}{cc}{b_{1}} & {b_{2}}\end{array}\right]\left[\begin{array}{c}{w_{1}} \\ {w_{2}}\end{array}\right] \\ &=\|g\| \overline{g}+b w \end{aligned}$ 既是：
$\begin{aligned} \hat{g} &=\|g\| \cdot \overline{\hat{g}}+\omega_{1} \overrightarrow{b_{1}}+\omega_{2} \overrightarrow{b_{2}} \\ &=\|g\| \cdot \hat{\hat{g}}+\vec{B} \vec{\omega} \end{aligned}, \quad B \in \mathbb{R}^{3 \times 2}, \vec{\omega} \in \mathbb{R}^{2 \times 1}\tag{34}$
$\overline{\hat{g}}$ 爲重力向量的方向向量在 $\mathcal{C}_{0}$ 座標系下的座標, $b_{1}, b_{2}$ 爲重力向量正切空間的一對正交基。重新參數化後的重力向量由 $\omega$ 控制。令 $\hat{g}=g^{c_{0}}$ 代入公式(28)得:
$\left[\begin{array}{cccc}{-I \Delta t_{k}} & {0} & {\frac{1}{2} R_{c_{0}}^{b_{x}} \Delta t_{k}^{2}} &{R_{c_{0}}^{b_{k}}\left(\overline{p}_{c_{k+1}}^{c_{0}}-\overline{p}_{c_{k}}^{c_{0}}\right)} \\ {-I} & {R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}}} & {R_{c_{0}}^{b_{k}} \Delta t_{k}} &{0} \end{array}\right] \left[\begin{array}{c}{v_{b_{k}}^{b_{k}}} \\ {v_{b_{k+1}}^{b_{k+1}}} \\ { \omega } \\ {s}\end{array}\right]$
$= \left[\begin{array}{c}{\alpha_{b_{k+1}}^{b_{k}}-p_{c}^{b}+R_{c_{0}}^{b_{k}} R_{b_{k+1}}^{c_{0}} p_{c}^{b}-\frac{1}{2} R_{c_{0}}^{b_{k}} \Delta t_{k}^{2}\|\mathrm{g}\| \cdot \overline{\mathbf{g}}} \\ {\beta_{b_{k+1}}^{b_{k}}-R_{c_{0}}^{b_{k}} \Delta t_{k}\|\mathrm{g}\| \cdot \overline{\widehat{\mathrm{g}}}}\end{array}\right]\tag{35}$

即: $H^{6 \times 9} X_{I}^{9 \times 1}=b^{6 \times 1}, \quad \omega^{2 \times 1}=\left[\omega_{1}, \omega_{2}\right]^{T}$ 這樣,可以用 Cholosky 分解下面方程求解 $X_{I}$ : $H^{T} H X_{I}=H^{T} b\tag{35}$
同樣通過 Cholosky 分解求得 $g^{c_{0}}$ ,即相機 $C_0$ 系下的重力向量。最後,通過將 $g^{c_{0}}$ 旋轉至慣性座標系(世界系)中的 z 軸方向 [0,0,1],可以計算第一幀相機繫到慣性系的旋轉矩陣 $q_{c_{0}}^{w}$ ,這樣就可以將所有變量調整至慣性世界系(水平座標系,z 軸與重力方向對齊)中。

/**vins_estimator/src/initial/initial_aligment.cpp
 * [RefineGravity 糾正重力向量和速度]
 * @param all_image_frame [description]
 * @param g               [description]
 * @param x               [description]
 */
//每次估計完g以後, 都會做一次歸一化使得其模值爲G, 所以可以理解爲這裏的refine其實是refine重力的方向, 同時refine其他估計量. 
//因爲方向基本不會差太多, w1,w2通常很小, 所以可以做這樣的近似.
void RefineGravity(map<double, ImageFrame> &all_image_frame, Vector3d &g, VectorXd &x)

VIN3.系統初始化