[从零写VIO|第七节]——VINS-Mono代码精简版代码详解——初始化1外参标定(内容|代码)

本节是VIO的重点，主要是VINS的鲁棒初始化：外参估计，重力估计对齐

1.大体流程

初始化主要分成2部分，第一部分是纯视觉SfM优化滑窗内的位姿，然后在融合IMU信息，按照理论部分优化各个状态量。
得到外参后可以进行初始化的，代码 在estimator::processImage()的最后部分，
相关代码如下：

    if (ESTIMATE_EXTRINSIC == 2) // 对外参一无所知    
    {        
        cout << "calibrating extrinsic param, rotation movement is needed" << endl;        
        if (frame_count != 0)        
        {
            // 得到给定两帧之间的对应特征点3D坐标            
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);            
            Matrix3d calib_ric;
            //pre_integrations[frame_count]->delta_q是使用imu预积分获取的旋转矩阵,calib_ric为要计算的相机到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[0] = calib_ric;                
                RIC[0] = calib_ric;                
                ESTIMATE_EXTRINSIC = 1; // 得到了外参，可对整个vins系统进行初始化            
            }        
        }    
    }

    if (solver_flag == INITIAL) // 判断系统是否已经进行初始化    
    {   
        //WINDOW_SIZE==10 frame_count达到了WINDOW_SIZE，这样能确保有足够的frame参与初始化        
        if (frame_count == WINDOW_SIZE)        
        {            
            bool result = false;
            //外参初始化成功且当前帧时间戳距离上次初始化时间戳大于0.1秒，就进行初始化操作            
            if (ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)            
            {                
                // cout << "1 initialStructure" << endl; 
                //对视觉和惯性单元进行初始化               
                result = initialStructure();                
                initial_timestamp = header;            
            }            
            if (result) // result==true,初始化完成——>非线性优化            
            {
                //先进行一次滑动窗口非线性优化，得到当前帧与第一帧的位姿                
                solver_flag = NON_LINEAR;                
                solveOdometry();                
                slideWindow();                
                f_manager.removeFailures();                
                cout << "Initialization finish!" << endl;                
                last_R = Rs[WINDOW_SIZE];                
                last_P = Ps[WINDOW_SIZE];                
                last_R0 = Rs[0];                
                last_P0 = Ps[0];            
            }            
            else                
                slideWindow(); //初始化失败则直接滑动窗口        
        }        
            else            
                frame_count++; //图像帧数量+1    
    }

其中，f_manager.getCorresponding()的作用是：得到给定两帧之间的对应特征点3D坐标，相关代码如下：

vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{    
    vector<pair<Vector3d, Vector3d>> corres;    
    for (auto &it : feature)// 遍历特征点feature的list容器    
    {        
        // 要找特征点的两帧在窗口范围内，可以直接取。窗口为：观测到当前特征点的所有图像帧        
        // start_frame首次被观测到时，该帧的索引        
        if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)        
        {            
            Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();            
            int idx_l = frame_count_l - it.start_frame;            
            int idx_r = frame_count_r - it.start_frame;
            a = it.feature_per_frame[idx_l].point;
            b = it.feature_per_frame[idx_r].point;
                                    
            corres.push_back(make_pair(a, b));        
        }    
    }    
    return corres;
}

VINS外参标定

VINS外参标定指的是对相机坐标系到IMU坐标系的变换矩阵进行在线标定与优化。

在参数配置文件yaml中，参数estimate_extrinsic反映了外参的情况：
1、等于0表示这外参已经是准确的了，之后不需要优化；
2、等于1表示外参只是一个估计值，后续还需要将其作为初始值放入非线性优化中；
3、等于2表示不知道外参，需要进行标定，从代码estimator.cpp中的processImage()中的以下代码进入，主要是标定外参的旋转矩阵。

solver_flag用于判断初始化是否完成，当其值为INITIAL的时候表示初始化未完成，初始化成功后solver_flag的值会修改为NON_LINEAR。

从上边代码可以看到，只有当滑动窗口中的图像帧个数达到WINDOW_SIZE=10后才会进行初始化操作。这里滑动窗口的大小设置为10帧，是为了限制计算复杂度，因为后边的旋转和平移计算是需要遍历滑动窗口中最后一帧之前的所有帧的。可以看到，调用initialStructure函数后，如果判断初始化失败了，就直接去滑动窗口（也就是根据策略删除窗口中的某一帧）。

【注意】初始化主要有以下几个问题：

• IMU怎么和世界坐标系对齐？
• 单目视觉姿态如何和IMU轨迹对齐，尺度如何获取？
• VIO系统的初始化速度v，传感器bias等如何估计？
• IMU和相机之间的外参数等如何估计？

vins的初始化细节的过程在initial文件夹里。

2. 若旋转外参数$q_{bc}$未知, 则先估计旋转外参数

估计旋转外参数分两步走：

2.1获取最新两帧之间匹配的特征点对

具体看getCorresponding函数中的处理。
具体地，对于每个特征，如果[frame_count - 1, frame_count]区间被包含在其[start_frame,endFrame()]中，那么获取这相邻两帧分别对应的观测点坐标，存储在corres中。代码在feature_manager.cpp中:

vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{    
    vector<pair<Vector3d, Vector3d>> corres;    
    for (auto &it : feature)    
    {   //判断每个特征点的对应的frame是否在有效范围内        
        if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)        
        {            
            Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();
            //计算传入的frame_count_l和frame_count_r距离最开始一帧的距离，也就是最新两帧的index            
            int idx_l = frame_count_l - it.start_frame;            
            int idx_r = frame_count_r - it.start_frame;
            
            a = it.feature_per_frame[idx_l].point;
            
            b = it.feature_per_frame[idx_r].point;
                                    
            corres.push_back(make_pair(a, b));        
        }    
    }
    //corres中存放的就是每一个特征it在两个帧中对应的point点对    
    return corres;
}

【补充】
make_pair 简单使用
cv::Point2f类型

2.2 计算相机-IMU之间的旋转

计算出最新两帧图像之间对应的匹配特征点对后，调用CalibrationExRotation根据匹配点对计算相对旋转。

关于相机和imu旋转外参数的估计方法有很多。除了即将要介绍的vins中使用的利用旋转约束去估计外，也可以使用calibr或者传感器器件的安装来标定这个参数。
利用旋转约束估计的方法：

可得公式：

bool CalibrationExRotation(vector> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result);

具体代码于：initial_ex_rotation.cpp 函数 CalibrationExRotation().详细解释见代码内。

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{   
    //! Step1：滑窗内的帧数加1    
    frame_count++;
    //! Step2:计算两帧之间的旋转矩阵    
    // 利用帧可视的3D点求解相邻两帧之间的旋转矩阵R_{ck,ck+1}    
    Rc.push_back(solveRelativeR(corres)); // 两帧图像之间的旋转通过solveRelativeR计算出本质矩阵E，再对矩阵进行分解得到图像帧之间的旋转R。     
    //delta_q_imu为IMU预积分得到的旋转四元数，转换成矩阵形式存入Rimu当中。则Rimu中存放的是imu预积分得到的旋转矩阵    
    Rimu.push_back(delta_q_imu.toRotationMatrix());     
    //每帧IMU相对于起始帧IMU的R,ric初始化值为单位矩阵，则Rc_g中存入的第一个旋转向量为IMU的旋转矩阵    
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric); // R_{bk,bk+1}
    
    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]); // q_{ck,ck+1}         
        Quaterniond r2(Rc_g[i]); // q_{bk,bk+1}
        
        //！Step3:求取估计出的相机与IMU之间旋转的残差 公式(9)        
        //！r = acos((tr(R_cbRbR_bcR_c)-1)/2)        
        double angular_distance = 180 / M_PI * r1.angularDistance(r2);        
        //ROS_DEBUG("%d %f", i, angular_distance);
        
        //! Step4:计算外点剔除的权重 Huber函数 公式(8)        
        double huber = angular_distance > 5.0 ? 5.0 / angul/ar_distance : 1.0; // 将阈值threshold取做5度        
        ++sum_ok;        
        Matrix4d L, R;
        
        //! Step5：求取系数矩阵        
        //! 得到相机对极关系得到的旋转q的左乘
        //四元数由q和w构成，q是一个三维向量，w为一个数值        
        double w = Quaterniond(Rc[i]).w();        
        Vector3d q = Quaterniond(Rc[i]).vec();
        // 四元数的左右乘见下面的知识点        
        //L为相机旋转四元数的左乘矩阵，Utility::skewSymmetric(q)计算的是q的反对称矩阵        
        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为IMU旋转四元数的右乘矩阵        
        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;
        // 公式(7)中的Q_N
        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-Camera的测量对数是否大于滑窗大小。    
    //！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;
}

【补充】

1. 计算 angle distance,目的是得到角度误差$\theta$,为将要求解的线性方程增加鲁棒核权重,它俩之间的角位移就是旋转向量对应的角度$\varphi$,也就是对应代码double angular_distance = 180 / M_PI * r1.angularDistance(r2);。angularDistance()的代码实现可以参考Eigen Geometry模块下的angularDistance()函数
   

2.求解两帧之间旋转矩阵的solveRelativeR()函数,具体实现代码以及相关知识点可见——slam实践：对极约束与三角测量
2. SVD分解的c++代码（Eigen 库）
3.

3. 计算出旋转外参数$q_{bc}$(对应代码中的ric)后，开始其他参数的初始化

其代码在estimator.cpp中的Estimator::initialStructure()。

后面内容见我的下一篇博客——指路