VINS-Mono代码阅读笔记（七）：vins_estimator中相机与IMU的外参标定

最新推荐文章于 2024-07-29 19:27:18 发布

文科升

最新推荐文章于 2024-07-29 19:27:18 发布

阅读量4.5k

点赞数 8

分类专栏： SLAM 文章标签： vins-mono vons slam

本文链接：https://blog.csdn.net/moyu123456789/article/details/102998854

版权

SLAM 专栏收录该内容

35 篇文章 90 订阅

订阅专栏

VINS SLAM系统运行过程中，其实就是根据一帧一帧的图像之间匹配的特征点，通过对极几何进行相机位姿的计算，IMU的加入能够将尺度极大的“精确化”，这样就使得相机的运动有了测量尺度。那么，系统中有了两个sensor，这两个sensor各自产生了自己的坐标系，要将两个坐标系下的数据联系起来就需要知道两个坐标系之间的旋转和平移关系。那么，单目相机和IMU之间的旋转关系也就是外参标定怎么来计算呢？本篇笔记主要分析这个问题。

1.相机与IMU外参标定是什么

VINS系统中使用了相机来获取图像信息，使用IMU来“获得”精确的尺度，那么当搭载VINS系统的设备（无人车、无人机等）运动的时候，相机中连续的每一帧图像之间通过匹配的特征点利用九点法可以计算出基础矩阵，进而通过svd分解可以得到旋转矩阵序列： $R_{c_1}^{c_0}$ ， $R_{c_2}^{c_1}$ ， $R_{c_3}^{c_2}$ …… $R_{c_{k+1}}^{c_k}$ …… 相应的IMU通过和图像对齐之后的预积分可以得到旋转矩阵序列： $R_{b_1}^{b_0}$ 、 $R_{b_2}^{b_1}$ 、 $R_{b_3}^{b_2}$ …… $R_{b_{k+1}}^{b_k}$ ……

相机与IMU之间的外参标定，就是计算相机与IMU之间的相对旋转将上边的{ $R_{c_1}^{c_0}$ ， $R_{c_2}^{c_1}$ ， $R_{c_3}^{c_2}$ …… $R_{c_{k+1}}^{c_k}$ }序列和{ $R_{b_1}^{b_0}$ ， $R_{b_2}^{b_1}$ ， $R_{b_3}^{b_2}$ …… $R_{b_{k+1}}^{b_k}$ ……}序列对应起来。由于搭载相机和IMU的设备（无人车或者无人机）是刚体，所以计算出来的旋转是个常数。

当计算出相对旋转后，就可以把相机和IMU之间的运动完美对应起来。

下图是港科大沈老师讲座中提到的相机和IMU之间旋转标定的图片。图中绿色的方块和绿色虚线表示IMU的运动旋转轨迹，蓝色图标和蓝色虚线表示相机的运动旋转轨迹。通过下面两张图片，可以很清楚的理解相机与IMU之间旋转的标定。

当计算出相机相对于IMU的旋转后，对相机的运动轨迹按照其相对于IMU的旋转进行旋转调整后，相机和IMU的运动轨迹就可以完美匹配在一起，如下图所示：

只有相机和IMU的运动相匹配了，IMU的测量值才对相机获取的每一帧图像有意义。

2.相机与IMU之间的旋转求解理论

这部分详细参见论文：Monocular Visual–Inertial State Estimation With Online Initialization and Camera–IMU Extrinsic Calibration中V-A部分的描述。

以下内容主要为该论文中相关部分的描述：

设相机与IMU之间的旋转为 R_c^b ，那么，以下公式成立：

$R_{b_{k+1}}^{b_k}\cdot R_c^b= R_c^b\cdot R_{c_{k+1}}^{c_k} . (4)$

注意：这里穿插一段，稍微说一下这个公式。这个其实就是手眼标定的问题，这个公式也是手眼标定当中著名的 AX=XB 方程。关于手眼标定的一些解释和推导参见手眼标定AX=XB原理。

这里仍然延用论文中的公式编号。这里可以看出左边是旋转右乘了IMU帧的旋转，右侧为旋转左乘了相机图像帧的旋转。使用四元数表达旋转，可以将（4）式写为：

$q_{b_{k+1}}^{b_k}\bigotimes q_c^b = q_c^b\bigotimes q_{c_{k+1}}^{c_k}$ $\Rightarrow \left [ Q_1(q_{b_{k+1}}^{b_k})-Q_2(q_{c_{k+1}}^{c_k}}) \right ]\cdot q_c^b=Q_{k+1}^{k}\cdot q_c^b= 0 .(5)$

上式中：

$Q_1(q)=\begin{bmatrix} q_wI_3+\left \lfloor q_{xyz}\times \right \rfloor & q_{xyz} \\ -q_{xyz} & q_w \end{bmatrix}$

$Q_2(q)=\begin{bmatrix} q_wI_3-\left \lfloor q_{xyz}\times \right \rfloor & q_{xyz} \\ -q_{xyz} & q_w \end{bmatrix}$

Q_1(q) 和 Q_2(q) 是四元素左乘和右乘的矩阵形式， q_w 是四元数的实部， $q_{xyz}$ 为虚部组成的向量， $\left \lfloor q_{xyz} \times \right \rfloor$ 是来自四元数 $q_{xyz}$ 的前三个元素组成的反对称矩阵。

相机中传入的连续的一帧一帧的图像，通过特征点匹配后的图像构成一对匹配图像，多对匹配图像可以构造如下的线性方程：

$\begin{bmatrix} w_1^0\cdot Q_1^0\\ w_2^1\cdot Q_2^1\\ ...\\ w_{N}^{N-1}\cdot Q_N^{N-1}\end{bmatrix}\cdot q_c^b=Q_N\cdot q_c^b. (7)$

这里的N表示最后一帧的索引值，并且一直增长直到旋转标定成功。论文中提到，这里需要注意从五点法算法获得的增量旋转测量值，包括由于退化运动情况下的错误的对应关系或者数值错误导致的异常点。所以这里引进了权重的概念，也就是上式中的 $w_N^{N-1}$ 这样的值，它与旋转求取过程中的残差有关，公式如下：

$r_{k+1}^k=acos((tr(\hat{R}_c^{b^{-1}}R_{b_{k+1}}^{{b_k}^{-1}}\hat{R}_c^bR_{c_{k+1}}^{c_k})-1)/2). (8)$

有了上边的式子，权重定义如下：

$w_{k+1}^{k}=\left\{\begin{matrix} 1, & r_{k+1}^k<threshold\\ \frac{threshold}{r_{k+1}^k}, &otherwise. \end{matrix}\right. (9)$

如果没有足够的特征来估计相机旋转，则 $w_{k+1}^k$ 设置为0。通过求解 $Q_N\cdot q_c^b=0$ 的解， Q_N 的最小奇异值对应的右奇异向量便为所求的旋转。

论文中还提到了求解相机-IMU相对旋转的终止标准，其实就是通过检查 Q_N 的第二小的奇异值是否大于设置的某个阈值，如果大于设置的阈值（代码中阈值是0.25）则认为外参标定的求解可以终止。

3.代码分析

对照上边论文中提到的理论，具体来看一下代码中是怎么写的。这一篇笔记是接着上一篇笔记 VINS-Mono代码阅读笔记（六）：vins_estimator中图像处理1的。所以，这里还是从processImage中代码开始分析，下面代码是processImage中进行外参标定的代码。

/**
     * 外参初始化
     * VINS外参标定指的是对相机坐标系到IMU坐标系的变换矩阵进行在线标定与优化。
    */
    if(ESTIMATE_EXTRINSIC == 2)
    {
        //标定外参，先进行初步估计初始化，后续优化的时候再进一步进行优化
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        if (frame_count != 0)
        {
            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，目前单目情况下，ric内只有一个值
                ric[0] = calib_ric;
                RIC[0] = calib_ric;
                //如果校准成功就设置flag为1
                ESTIMATE_EXTRINSIC = 1;
            }
        }
    }

总共分为两步：

第一步，获取最新两帧之间匹配的特征点对。这个具体看getCorresponding函数中的处理，还是比较简单的。

/**
 * 函数功能：找出最新的两帧之间共视特征点的对应points有哪些
 * 传入的参数frame_count_l 为frame_count-1
 * frame_count_r为frame_count
*/
vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{
    //存储3d点对的容器列表
    vector<pair<Vector3d, Vector3d>> corres;
    //遍历feature list中每一个特征
    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中存放的就是每一个特征it在两个帧中对应的point点对
            corres.push_back(make_pair(a, b));
        }
    }
    return corres;
}

其中feature的数据结构图如下所示：

第二步，计算相机-IMU之间的旋转。计算出最新两帧图像之间对应的匹配特征点对后，调用CalibrationExRotation根据匹配点对计算相对旋转。CalibrationExRotation函数代码如下，可以把该函数中的代码和上边提到的理论进行对比分析来增强对代码的理解。

/**
 * 标定相机和IMU之间的外参旋转矩阵
 * delta_q_imu是IMU预积分得到的旋转矩阵，会和视觉跟踪求解fundamentalMatrix分解后获得的旋转矩阵构建约束方程，从而标定出外参旋转矩阵
*/
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++;
    //solveRelativeR函数中可以根据corres中特征点的对应关系计算出前后两帧图像之间的旋转矩阵，加到Rc中，Rc表示相机位姿的旋转矩阵
    Rc.push_back(solveRelativeR(corres));
    //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);

    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);
        ROS_DEBUG(
            "%d %f", i, angular_distance);
        //这里将阈值threshold取做5度
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R;
        //四元数由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;

        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;

        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }
    //进行SVD分解
    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;
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();
    //迭代计算了>=WINDOW_SIZE次并且ric_cov(1) > 0.25，则认为外参标定成功
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}

这里对svd分解做个简单介绍：SVD分解的c++代码（Eigen 库）

其中，两帧图像之间的旋转通过solveRelativeR计算出本质矩阵，再对矩阵进行分解得到图像帧之间的旋转。

/**
 * 通过计算匹配特征点之间的本质矩阵得到旋转矩阵
*/
Matrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres)
{
    if (corres.size() >= 9)
    {
        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)));
        }
        //计算本质矩阵
        cv::Mat E = cv::findFundamentalMat(ll, rr);
        cv::Mat_<double> R1, R2, t1, t2;
        //从本质矩阵中解析获得旋转矩阵和平移向量
        decomposeE(E, R1, R2, t1, t2);

        if (determinant(R1) + 1.0 < 1e-09)
        {
            E = -E;
            decomposeE(E, R1, R2, t1, t2);
        }
        //找到适当的R、t
        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;

        Matrix3d ans_R_eigen;
        //将ans_R_eigen进行了转置操作，得到的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();
}

processImage中下面两行代码，即表示求解的单目相机-IMU之间的旋转矩阵存储在ric[0]、RIC[0]中，有了这个旋转矩阵就能将相机的运动和IMU的运动对应起来了，后边注意分析怎样使用这个值。