VINS 细节系列 - 外参在线标定

一、概念

所谓的外参是从 相机（Camera）坐标系 到 IMU坐标系 的相对旋 Rbc ，在该函数中用的ric表示的；

其他过多的就不解释了，具体看我的博客：https://blog.csdn.net/hltt3838/article/details/109356748

 

二、原理

这里求解外参用的方法是最小二乘法，构建 AX = 0 的形式，然后使用奇异值分解（SVD）对 A 矩阵进行分解，

最小奇异值对应的右向量就是 AX = 0 的非零解。下面就是构建最小二乘形式的过程:

上式可写成:

疑问：为什么公式（7）采用SVD求解，最小奇异值对应的奇异向量就是方程的解？

答：请看这个连接！     https://blog.csdn.net/hltt3838/article/details/108834711

 

三、代码

 if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))

函数的入参 corres 是包含 两帧之间 的多个匹配点对的归一化坐标的vector数组，入参 delta_q 是通过 两帧间 陀螺仪积分得到相对旋转；

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> 
     corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    /*记录第几次进入这个函数，标定的过程中会多次进入这个函数，
    直到标定成功，每进一次通过入参得到公式（6）这样一个约束。*/
    frame_count++;  
   /*过帧间的匹配点对计算得到本质矩阵，然后分解得到旋转矩阵R_ck+1^ck*/
    Rc.push_back(solveRelativeR(corres));
    Rimu.push_back(delta_q_imu.toRotationMatrix());//帧间陀螺仪积分得到R_bk+1^bk
    /*每次迭代前先用前一次估计的ric将R_bk+1^bk变换成R_ck+1^ck，即公式（1）*/
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);

    Eigen::MatrixXd A(frame_count * 4, 4); //构建公式（7）中的A矩阵
    A.setZero();
    int sum_ok = 0;
    for (int i = 1; i <= frame_count; i++)
    {
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);

        /* angularDistance就是计算两个坐标系之间相对旋转矩阵在做轴角变换后(u * theta)
        的角度theta, theta越小说明两个坐标系的姿态越接近，这个角度距离用于后面计算权重，
        这里计算权重就是为了降低外点的干扰，意思就是为了防止出现误差非常大的R_bk+1^bk和 
        R_ck+1^ck约束导致估计的结果偏差太大*/
        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        ROS_DEBUG(
            "%d %f", i, angular_distance);
//核函数做加权
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;

   
        /*L R 分别为四元数左乘和四元数右乘矩阵
         下面的这几步就是从公式（3）到公式（6）的过程 ，每次得到4行约束填入A矩阵中*/
        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]);
        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.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }

    //对A矩阵做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();
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();

    /* 至少会迭代WINDOW_SIZE次，并且第二小的奇异值大于0.25才认为标定成功
   （singularValues拿到的奇异值是从大到小存储的），
    意思就是最小的奇异值要足够接近于0，和第二小之间要有足够差距才行, 
    这样才算求出了最优解 */
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}