本講還是爲了estimator類中最主要的函數processImage（）做知識儲備。

前面兩講知識儲備主要講了IMU預積分相關的integrationBase類以及圖像特徵點管理器feature_manager.cpp，本節將對processImage（）的相機到IMU的外參矩陣的代碼講解。

當外參完全不知道的時候，VINS也可以在線對其進行估計（rotation），先在processImage內進行初步估計，然後在後續優化時，會在optimize函數中再次優化。

InitialEXRotation類

1、CalibrationExRotation（）函數

1.初始化獲取幾個旋轉矩陣，放入到vector中

2、SVD分解，Ax=0,對A填充。多幀組成A,一對點組成的A是4*4的。

3.至少迭代計算了WINDOW_SIZE次，且R的奇異值大於0.25才認爲標定成功

2、solveRelativeR（）

3、decomposeE（）

4、testTriangulation（）

processImage（）中：

第一步爲檢測當前輸入的image是否爲關鍵幀，f_manager.addFeatureCheckParallax（）

第二步爲更新臨時預積分初始值 tmp_pre_integration = new IntegrationBase（）

第三步爲：本文的貢獻中相機和IMU外參的在線標定作爲創新點。initial_ex_rotation.CalibrationExRotation（）代碼如下：

// 3. 如果沒有外參則標定IMU到相機的外參
// 0表示外參優化完成；1表示粗略估計，後面作爲初始值放進非線性優化；2表示需要進行標定。
    if(ESTIMATE_EXTRINSIC == 2)
    {
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        if (frame_count != 0)
        {
            //得到corres傳入的是當前幀和其之前一幀的對應特徵點對的歸一化座標。
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            Matrix3d calib_ric;
            //標定從camera到IMU之間的外參數
            if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
            {
                ROS_WARN("initial extrinsic rotation calib success");
                ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
                //有幾個相機，就有幾個ric，目前單目情況下，ric內只有一個值
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                ESTIMATE_EXTRINSIC = 1;// 完成外參標定
            }
        }
    }

InitialEXRotation類

位於vins_estimator/src/initial/initial_ex_rotation.cpp，首先介紹一個這個類的成員變量以及成員函數：

/* 標定IMU和相機的外參數 */
class InitialEXRotation
{
public:
	InitialEXRotation();
    // ！！！重要，主函數標定外參旋轉矩陣
    bool CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);
private:
    // 兩幀之間相機系旋轉矩陣 
	Matrix3d solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres);

    double testTriangulation(const vector<cv::Point2f> &l,
                             const vector<cv::Point2f> &r,
                             cv::Mat_<double> R, cv::Mat_<double> t);
    // 本質矩陣SVD分解求得4組RT                         
    void decomposeE(cv::Mat E,
                    cv::Mat_<double> &R1, cv::Mat_<double> &R2,
                    cv::Mat_<double> &t1, cv::Mat_<double> &t2);
    int frame_count;

    vector< Matrix3d > Rc;// 相機之間旋轉矩陣，對極幾何得到
    vector< Matrix3d > Rimu;// 兩個IMU之間旋轉矩陣，由IMU預積分得到
    vector< Matrix3d > Rc_g;  // 每個IMU相對於起始IMU的旋轉矩陣
    Matrix3d ric;// 相機到IMU的外參
};

1、CalibrationExRotation（）函數

標定相機和IMU的外參數。

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)

輸入參數爲：vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, 匹配的特徵點和 IMU預積分得的旋轉矩陣Q

輸出參數：Matrix3d &calib_ric_result 標定的外參數

1.初始化獲取幾個旋轉矩陣，放入到vector中

記錄第幾次進入這個函數，標定的過程會多次進入到這個約束，直到標定成功，每進一次都會得到公式的約束。

frame_count++;

計算前後兩幀的旋轉矩陣，加到Rc向量內。

相機幀之間匹配點得到本質矩陣，分解得到旋轉矩陣R_ck+1^ck

IMU之間預積分得到旋轉矩陣R_bk+1^bk

每次迭代之前先用上一次估計的ric將R_bk+1^bk轉換爲R_ck+1^ck，爲了下面求解核函數。

    Rc.push_back(solveRelativeR(corres));// 相機幀之間旋轉矩陣
    Rimu.push_back(delta_q_imu.toRotationMatrix());//IMU之間旋轉矩陣，由IMU預積分得到
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//每幀IMU相對於起始幀IMU的R，初始化爲IMU預積分

2、旋轉約束估計外參數 $q_{b}^{c}$

SVD分解，Ax=0,對A填充。多幀組成A,一對點組成的A是4*4的。

機器人手眼標定的方法計算出外參的矩陣。

其中， $q_{b_{k+1}}^{b_{k}}$ 是IMU預積分得到的， $q_{c_{k+1}}^{c_{k}}$ 是用8點法對前後幀的特徵點計算得到的。

詳細見《Monocular Visual-Inertial State Estimation With Online Initialization and Camera-IMU Extrinsic Calibration》

從 $b_{k}$ 到 $c_{k+1}$ 有兩條路徑：第一條是從 $b_{k}\rightarrow b_{k+1 }\rightarrow c_{k+1 }$ ，對應綠色部分。

第二條是 $b_{k}\rightarrow c_{k}\rightarrow c_{k+1 }$ ，對應紅色部分。

核函數說明：

angularDistance就是計算兩個座標系之間相對旋轉矩陣在做軸角變換後(u * theta)
的角度theta, theta越小說明兩個座標系的姿態越接近，這個角度距離用於後面計算權重，
這裏計算權重就是爲了降低外點的干擾，意思就是爲了防止出現誤差非常大的R_bk+1^bk和
R_ck+1^ck約束導致估計的結果偏差太大

這裏權重用來抑制過大殘差值的影響，表現爲 $w_{k+1}^{k}$

殘差 $r_{k+1}^{k}$ 是兩條路徑構建的誤差。

 // 2.1求解核函數
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);
        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        ROS_DEBUG(
            "%d %f", i, angular_distance);
        //huber核函數做加權
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;

對L和R賦值的情況做以下說明：

四元數左乘（最後一列是b）：對應相機L

四元數右乘（最後一列是a）：對應IMU

    // 2.SVD分解，Ax=0,對A填充
    Eigen::MatrixXd A(frame_count * 4, 4);// 多幀組成A,一對點組成的A是4*4的。
    A.setZero();
    int sum_ok = 0;
    for (int i = 1; i <= frame_count; i++)
    {
        // 2.1求解核函數
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);
        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        ROS_DEBUG(
            "%d %f", i, angular_distance);
        //huber核函數做加權
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        // 2.2 分別爲L和R賦值，L R 分別爲左乘和右乘矩陣，都是4*4矩陣
        //R_bk+1^bk * R_c^b = R_c^b * R_ck+1^ck
        //[Q1(q_bk+1^bk) - Q2(q_ck+1^ck)] * q_c^b = 0
        Matrix4d L, R;

        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;

        Quaterniond R_ij(Rimu[i]);// IMU間旋轉矩陣
        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;
        // A=Q1(q_bk+1^bk) - Q2(q_ck+1^ck)=huber(L-R)
        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }

    // 2.3 svd分解中最小奇異值對應的右奇異向量作爲旋轉四元數
    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    //這裏的四元數存儲的順序是[x,y,z,w]',即[qv qw]'
    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;
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();

參考：https://zhuanlan.zhihu.com/p/108658768

3.至少迭代計算了WINDOW_SIZE次，且R的奇異值大於0.25才認爲標定成功

if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;

2、solveRelativeR（）

根據兩幀歸一化特徵點求解兩幀的旋轉矩陣

//根據兩幀歸一化特徵點求解兩幀的旋轉矩陣
Matrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres)
{
    if (corres.size() >= 9)// 需要特徵點大於9對，否則返回單位矩陣
    {
        // 歸一化相機系下前二維座標SVD分解
        vector<cv::Point2f> ll, rr;
        for (int i = 0; i < int(corres.size()); i++)
        {
            ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
            rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
        }
        // 求解兩幀的本質矩陣E
        cv::Mat E = cv::findFundamentalMat(ll, rr);
        cv::Mat_<double> R1, R2, t1, t2;
        // 本質矩陣svd分解得到4組Rt解
        decomposeE(E, R1, R2, t1, t2);
        // 如果行列式爲負，SVD分解-E
        if (determinant(R1) + 1.0 < 1e-09)
        {
            E = -E;
            decomposeE(E, R1, R2, t1, t2);
        }

        // 通過三角化得到的正深度選擇Rt解
        double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
        double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));
        cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;

        // 對R求轉置
        Matrix3d ans_R_eigen;
        for (int i = 0; i < 3; i++)
            for (int j = 0; j < 3; j++)
                ans_R_eigen(j, i) = ans_R_cv(i, j);
        return ans_R_eigen;
    }
    return Matrix3d::Identity();
}

3、decomposeE（）

void InitialEXRotation::decomposeE(cv::Mat E,
                                 cv::Mat_<double> &R1, cv::Mat_<double> &R2,
                                 cv::Mat_<double> &t1, cv::Mat_<double> &t2)
{
    cv::SVD svd(E, cv::SVD::MODIFY_A);
    cv::Matx33d W(0, -1, 0,
                  1, 0, 0,
                  0, 0, 1);
    cv::Matx33d Wt(0, 1, 0,
                   -1, 0, 0,
                   0, 0, 1);
    R1 = svd.u * cv::Mat(W) * svd.vt;
    R2 = svd.u * cv::Mat(Wt) * svd.vt;
    t1 = svd.u.col(2);
    t2 = -svd.u.col(2);
}

4、testTriangulation（）

double InitialEXRotation::testTriangulation(const vector<cv::Point2f> &l,
                                          const vector<cv::Point2f> &r,
                                          cv::Mat_<double> R, cv::Mat_<double> t)
{
    cv::Mat pointcloud;
    cv::Matx34f P = cv::Matx34f(1, 0, 0, 0,
                                0, 1, 0, 0,
                                0, 0, 1, 0);
    cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0),
                                 R(1, 0), R(1, 1), R(1, 2), t(1),
                                 R(2, 0), R(2, 1), R(2, 2), t(2));
    cv::triangulatePoints(P, P1, l, r, pointcloud);// 三角化得到路標3D座標
    int front_count = 0;
    for (int i = 0; i < pointcloud.cols; i++)
    {
        double normal_factor = pointcloud.col(i).at<float>(3);

        cv::Mat_<double> p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor);
        cv::Mat_<double> p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor);
        if (p_3d_l(2) > 0 && p_3d_r(2) > 0)
            front_count++;
    }
    ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols);
    return 1.0 * front_count / pointcloud.cols;
}

VINS-Mono 代碼詳細解讀——基礎儲備：在線Cam到IMU的外參標定 InitialEXRotation類

InitialEXRotation類

1、CalibrationExRotation（）函數

1.初始化獲取幾個旋轉矩陣，放入到vector中

2、旋轉約束估計外參數 $q_{b}^{c}$

3.至少迭代計算了WINDOW_SIZE次，且R的奇異值大於0.25才認爲標定成功

2、solveRelativeR（）

3、decomposeE（）

4、testTriangulation（）

VINS-Mono 代碼詳細解讀——初始化1：視覺SFM詳解 processImage（）+initialStructure（）

多傳感器融合定位2（封裝KF和EKF）

Endnote連接Word自動插入畢業論文參考文獻

《wiki官網教程》2 編寫簡單的服務器service和客戶端 client(C++)

VINS-Mono 代碼詳細解讀——基於滑動窗口的緊耦合後端非線性優化 optimization()+滑動窗slideWindow()

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結