Vins-mono 源码笔记 (4) 在线标定

1、在线外参标定

代码如下:

// 相机与IMU外参的在线标定
// 初始化成功 
if(ESTIMATE_EXTRINSIC == 2)                     // 不知道外参时     
{
    ROS_INFO("calibrating extrinsic param, rotation movement is needed");
    if (frame_count != 0)
    {
     // 获得前后两帧的特征点的关联     pair<Vector3d, Vector3d>表示同一特征点在两帧归一化平面上的坐标  
        vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
        Matrix3d calib_ric;
     // CalibrationExRotation要完成至少要10帧  由于第一帧不要  所以就是取滑动窗口中 1-11帧
        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;           // 知道了外参    后面继续优化   
        } 
    }
}

总结:
1、从第二帧图像帧开始 (frame_count >= 1),开始获取与上一帧的匹配特征点 vector<pair<Vector3d, Vector3d>> corres, pair前后两个元素分别表示特征点在前一帧与当前帧的归一化平面坐标,也就是(0-1) …(10-11)帧之间的匹配

2、然后将该匹配corres与IMU预积分数据一同传入标定函数计算。
initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric)

CalibrationExRotation()

外参标定的实现函数.
这里要注意, 每次执行 CalibrationExRotation()时, 都会将之前参与标定计算的所有帧全部重新组合成最小二乘问题计算, 且每次标定,都会将当前数据保存在Rc Rimu Rc_g容器,下次标定时又重新取出.
这里的权重 huber 的设定是,之前求出来的 ric 误差越大, 则当前数据的权重越小, 但是感觉不合理, 应该说,之前数据求出来的 ric 误差越大,说明应该越相信当前数据,而不相信之前的数据(这样感觉也有问题, 因为随着误差减小, 权重也会减小, ric会难以收敛到最小值)作者这里的意思可能是, 一开始ric误差很大,权重很小, 然后随着计算误差慢慢变小,然后权重相对越来越大, 最后获得一个最佳值.

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    // 标定不成功 则该函数会反复执行     
    frame_count++;    // 初始化为0  
    Rc.push_back(solveRelativeR(corres));                                   // 将两帧之间的关联特征点传入  solveRelativeR计算旋转      
    Rimu.push_back(delta_q_imu.toRotationMatrix());                         // 将旋转四元数转为旋转矩阵  
    // 用来 求权重的
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);                      // 利用上一次求出的ric  利用IMU计算的旋转推算相机旋转的预测值    Qck,ck+1 '  
    // 构建A  超定方程
    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]);                // IMU推算的视觉旋转           

        double angular_distance = 180 / M_PI * r1.angularDistance(r2);        // 四元数残差转为角度   残差即  ric.inverse() * delta_q_imu * ric*Qck,ck+1^-1  
        ROS_DEBUG(
            "%d %f", i, angular_distance);
        // 阈值为5   若误差大于5 越大  则 huber 越小   
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        //        ++sum_ok;    
	    // 注意下面依据的方程是   Qcb × Qbk,bk+1 = Qck,ck+1 × Qcb     和论文刚好相反
	    // 四元数左乘向量与右乘向量  
        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;
        // IMU的预积分旋转  
        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;
        // 构造A矩阵     
        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }
     // 超定方程 Ax=0  求解最小二乘问题   
     // 对A进行svd分解    A = U*W*Vt
    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    Matrix<double, 4, 1> x = svd.matrixV().col(3);      // 取最后一列右奇异向量             
    Quaterniond estimated_R(x);              // 构造为四元数    表示qci
    ric = estimated_R.toRotationMatrix().inverse();     // 取逆   变成 ric   
    //cout << svd.singularValues().transpose() << endl;                                              
    //cout << ric << endl;
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();     // 提取  倒数3个奇异值   (1,2,3)
    // 标定完成的条件：   1、一个完整滑动窗口的数据都添加了    2、 倒数第二个奇异值是否大于0.25    
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}

问题:
为啥要求AX=0 , A的第二奇异值要足够大才认为求解有效???

2 在线时间戳标定

由于IMU与相机的数据传输, 以及曝光时间等的影响, 物理世界中相同时刻的IMU相机帧数据, 在实际使用的时候可能会有一个时间上的偏移, 这会影响VIO系统的性能.
解决办法简单说就是, 计算出每个特征点像素的移动速度, 然后根据速度值提前补偿时间戳不对齐对图像特征点的影响. 将补偿后的特征点坐标用作重投影误差的构建.