VINS-Mono 代码详细解读——基础储备：在线Cam到IMU的外参标定 InitialEXRotation类

本讲还是为了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()函数流程 

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    标定的外参数