image-20220630110823250

为什么需要进行初始化?

VINS 是一个单目紧耦合VIO, 是一个高度非线性的系统,而非线性系统优化问题是以初始值为参考的,如果初始值不合理,我们可能会陷入局部最小值抑或是增加更多的迭代次数;另外,VINS 采用的是单目VIO,所以存在尺度不确定性的问题,IMU的测量又存在偏置误差,如果没有良好的初始值很难将这两种测量结果有机融合,因而初始化是VIO最脆弱的步骤。综上所述,进行初始化是十分有必要的。

如何进行初始化?

论文中提到,单目紧耦合VIO 是一个高度非线性的系统,一开始就需要一个准确的初始值。我们通过将 IMU 预集成与纯视觉结构松耦合对齐来获得必要的初始值。

首先是用 $\mathrm{SFM}$ 得到纯视觉系统的初始化,即求解窗口内所有帧的位姿,和所有的路标点三维坐标;然后跟 $\mathrm{IMU}$ 预积分的值对齐,求解重力矢量、尺度因子、陀螺仪 $bais$ 以及每一帧对应的速度


在对VINS的前端视觉处理的基础上,本节开始进入初始化章节,学习前端的代码之后,不难发现,前端的很多程序都是取自集成的函数,重点了解VINS对图像的处理步骤更为关键。对于初始化而言,它是保证整个系统正确运行的基础,是单目相机和IMU有机融合的前提,其重要程度不言而喻。

旋转外参初始化

VINS只对旋转进行外参的标定,不对平移进行标定,在VINS代码中,使用标志位 ESTIMATE_EXTRINSIC 对外参是否进行标定进行判断:

ESTIMATE_EXTRINSIC == 0:表示这外参已经是准确的了,之后不需要优化;

ESTIMATE_EXTRINSIC == 1 :表示外参只是一个估计值,后续还需要将其作为初始值放入非线性优化中

ESTIMATE_EXTRINSIC == 2:表示不知道外参,需要进行标定,主要是标定外参的旋转矩阵。

外参初始化流程

如图所示,外参标定主要分为两个主要的步骤,①:得到当前帧与前一帧之间归一化特征点corres,②:将两帧之间的归一化特征点对corres传入标定外参旋转矩阵的函数CalibrationExRotation进行外参标定;

VINS外参标定

详细地说,每个路标点可以被多个图像帧观察到,比如图中的路标点 3,它可以被图像帧2,3,4,5所观察到,其中我们称首先看到该路标点的图像帧为起始帧,称最后一次观察到该路标点的图像帧为结束帧,然后我们可以通过对极约束求取相机帧间约束R,通过之前的IMU预积分,可以推导出IMU之间的帧间约束R;

求解外参的原理

求解相机到IMU的外参旋转矩阵

帧间cam的R,由对极几何得到:

这里使用四元数表示,Camera帧间约束为:$\mathbf{q}^{c_k}_{c_{k+1} } $,IMU的帧间约束为:$\mathbf{q}^{c_k}_{c_{k+1} } $,设IMU坐标系到相机坐标系之间的外参为:$\mathbf{q}_b^c$;因此可以得到:

$$ \mathbf{q}_b^c\otimes\mathbf{q}^{b_k}_{b_{k+1} } =\mathbf{q}^{c_k}_{c_{k+1} }\otimes\mathbf{q}_b^c\tag{1} $$

由四元数的左乘和右乘矩阵可得:

$$ \begin{align*} \mathcal{R}[\mathbf{q}^{b_k}_{b_{k+1} }]\cdot\mathbf{q}_b^c -\mathcal{L}[\mathbf{q}^{c_k}_{c_{k+1} }]\cdot\mathbf{q}_b^c&=0\\ \left\{ \mathcal{R}[\mathbf{q}^{b_k}_{b_{k+1} }]-\mathcal{L}[\mathbf{q}^{c_k}_{c_{k+1} }] \right\}\cdot\mathbf{q}_b^c&=0 \end{align*}\tag{2} $$

设一共由 $N$ 组帧间约束,则:

$$ \begin{align*} \left\{ \mathcal{R}[\mathbf{q}^{b_k}_{b_{k+1} }]_1-\mathcal{L}[\mathbf{q}^{c_k}_{c_{k+1} }]_1 \right\}\cdot\mathbf{q}_b^c&=0\\ \left\{ \mathcal{R}[\mathbf{q}^{b_k}_{b_{k+1} }]_2-\mathcal{L}[\mathbf{q}^{c_k}_{c_{k+1} }]_2 \right\}\cdot\mathbf{q}_b^c&=0\\ ···\\ \left\{ \mathcal{R}[\mathbf{q}^{b_k}_{b_{k+1} }]_N-\mathcal{L}[\mathbf{q}^{c_k}_{c_{k+1} }]_N \right\}\cdot\mathbf{q}_b^c&=0\\ \end{align*}\tag{3} $$

此处,可以构造一个矩阵乘法,设

$$ A= \begin{bmatrix} \mathcal{R}[\mathbf{q}^{b_k}_{b_{k+1} }]_1-\mathcal{L}[\mathbf{q}^{c_k}_{c_{k+1} }]_1\\ \mathcal{R}[\mathbf{q}^{b_k}_{b_{k+1} }]_2-\mathcal{L}[\mathbf{q}^{c_k}_{c_{k+1} }]_2\\ ···\\ \mathcal{R}[\mathbf{q}^{b_k}_{b_{k+1} }]_N-\mathcal{L}[\mathbf{q}^{c_k}_{c_{k+1} }]_N\\ \end{bmatrix}_{4N\times4}, X=[\mathbf{q}_b^c]_{4\times1}\tag{4} $$

由于矩阵 $A$ 中的参数都可以通过IMU与相机约束得到(其中,相机的帧间约束,通过对极几何求得,而IMU的这件约束,通过IMU预积分得到),因此,最终就可以求解矩阵方程,从而求得 $\mathbf{q}_b^c$;

求解矩阵方程,作者采用的是 SVD分解,最终通过最小二乘迭代,求得外参的最优值;

已知方程$Ax = 0$ ,对 $A$ 做SVD分解,得 $U\Sigma V^T x=0$,因为是超定方程,一般无法等于0,问题转换为求 $\arg\min\xi=\parallel U\Sigma V^T x \parallel_{min}$

因为 $U$ 是一个正交矩阵:正交变换不改变矩阵的秩、特征值、行列式、迹 。

所以有:

$$ \parallel Ax\parallel=\parallel U \Sigma V^Tx\parallel=\parallel \Sigma V^Tx\parallel\tag{5} $$

令 $y=V^T x$,则 $\parallel \Sigma V^Tx\parallel=\parallel \Sigma y \parallel$,做进一步处理,可以转换为最小化 $\parallel y^T\Sigma^T \Sigma y\parallel$

$$ y^T\Sigma^T \Sigma y=\sigma_1^2y^2_1+···+\sigma_n^2y^2_n,\ \sigma_1\geq\sigma_2\geq···\geq\sigma_n\geq0\tag{6} $$

约束条件是 $\parallel y\parallel=1$,因此,$y=\begin{bmatrix}0&0&···&1\end{bmatrix}^T$是最小解。

所以:

$$ x=Vy=\begin{bmatrix}V_1&V_2&···&V_n\end{bmatrix}\begin{bmatrix}0\\0\\···\\1\end{bmatrix}=V_n\tag{7} $$

即 $Ax=0$ 的SVD解是 $V$ 的最后一列。

相机帧间约束的求取

根据流程图,我们可以了解到使用对约束求解相机帧间约束的具体过程。首先,将每组相邻帧的像素坐标分别存储ll rr ,当然,为了保证算出的外参的高置信度,所以匹配点必须大于9(这里为了举例方便,就存放一组)。然后,使用对极几何求解这两帧的本质矩阵E,然后对本质矩阵进行 svd 分解得到 4 组 Rt 解,采用三角化对每组 Rt 解进行验证,选择是正深度的 Rt 解,由于这里的 R 是上一帧到当前帧的变换矩阵,对其求转置为当前帧相对于上一帧的姿态。

对应程序

外参初始化

image-20220630111816610

//P_ Step2 外参初始化
// VINS外参标定指的是对相机坐标系到IMU坐标系的变换矩阵进行在线标定与优化
if (ESTIMATE_EXTRINSIC == 2) //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;
        // 标定从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[0] = calib_ric;
            RIC[0] = calib_ric;
            ESTIMATE_EXTRINSIC = 1; //1表示外参只是一个估计值,后续还需要将其作为初始值放入非线性优化中;
        }
    }
}

求解共视帧

image-20220630111836260

这里主要使用函数getCorresponding求解具有共视点的两帧,并分别求取共视的路标点在两个帧中各自的坐标,然后将路标点在两帧中的坐标点$a,b$结成一对,为后面求解外参做铺垫。

/**
 * @brief 得到同时被 frame_count_l frame_count_r帧看到的特征点在各自帧中的坐标
 * @param frame_count_l 
 * @param frame_count_r 
 * @return vector<pair<Vector3d, Vector3d>> 
 */
vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{
    vector<pair<Vector3d, Vector3d>> corres;
    for (auto &it : feature) // 遍历所有路标点
    {
        // 保证需要的路标点被这两帧都观察到
        if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)
        {
            Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();
            // 获得在feature_per_frame中的索引
            // 因为feature_per_frame是一个vector,里面有多个frame,使用当前frame的索引减去起始帧,就可以得到在vector中的索引
            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;  // 路标点在帧l中的坐标
            b = it.feature_per_frame[idx_r].point;  // 路标点在帧r中的坐标
          
            corres.push_back(make_pair(a, b)); // 得到一对共视点
        }
    }
    return corres;
}

标定外参旋转矩阵

在前面,我们使用函数getCorresponding已经求解了共视点在两帧之间的坐标,然后就可以使用函数CalibrationExRotation进行外参的标定了,根据上面推导的求解外参公式可知,需要相机帧间约束$\mathbf{q}^{c_k}_{c_{k+1} } $,以及IMU帧间约束 $\mathbf{q}^{b_k}_{b_{k+1} }$,首先是相机的帧间约束,我们通过对极约束求解,而IMU之间的帧间约束,则由pre_integrations[frame_count]->delta_q给出。下面函数主要是利用两个帧间约束求解外参$\mathbf{q}^c_b$的过程。

image-20220630112754804

/**
 * @brief 标定外参旋转矩阵
 * 
 * @param corres 
 * @param delta_q_imu 
 * @param calib_ric_result 
 * @return true 
 * @return false 
 */
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++;
    Rc.push_back(solveRelativeR(corres));              //帧间cam的R,由对极几何得到
    Rimu.push_back(delta_q_imu.toRotationMatrix());    //帧间IMU的R,由IMU预积分得到
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric); //每帧IMU相对于起始帧IMU的R (ric是上一次求解得到的外参)

    // 求解相机到IMU的外参旋转矩阵
    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]);

        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        ROS_DEBUG(
            "%d %f", i, angular_distance);

        //huber核函数做加权
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        ++sum_ok;
        Matrix4d L, R; // 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;

        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);
    }
    //  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;

    // 至少迭代计算了 WINDOW_SIZE 次,且 R 的奇异值大于 0.25 才认为标定成功
    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;
}

对极几何求解相机帧间约束

image-20220630112903419

Matrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres)
{
    if (corres.size() >= 9) // 匹配点云越多,算出结果的置信度越高
    {
        // 将 corres 中对应点的二维坐标分别存入 ll 与 rr 中。
        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);

        // 对本质矩阵进行 svd 分解得到 4 组 Rt 解
        cv::Mat_<double> R1, R2, t1, t2;
        decomposeE(E, R1, R2, t1, t2);

        // 旋转矩阵的行列式应该是1  这里如果是-1就取一下反
        if (determinant(R1) + 1.0 < 1e-09)
        {
            E = -E;
            decomposeE(E, R1, R2, t1, t2);
        }
        // 采用三角化对每组 Rt 解进行验证,选择是正深度的 Rt 解
        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;

        // 这里的 R 是上一帧到当前帧的变换矩阵,对其求转置为当前帧相对于上一帧的姿态。
        Matrix3d 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();
}

本质矩阵的分解

void InitialEXRotation::decomposeE(cv::Mat E,
                                   cv::Mat_<double> &R1, cv::Mat_<double> &R2,
                                   cv::Mat_<double> &t1, cv::Mat_<double> &t2)
{
    cv::SVD svd(E, cv::SVD::MODIFY_A);
    cv::Matx33d W(0, -1, 0,
                  1, 0, 0,
                  0, 0, 1);
    cv::Matx33d Wt(0, 1, 0,
                   -1, 0, 0,
                   0, 0, 1);
    R1 = svd.u * cv::Mat(W) * svd.vt;
    R2 = svd.u * cv::Mat(Wt) * svd.vt;
    t1 = svd.u.col(2);
    t2 = -svd.u.col(2);
}

三角化来检查外参是否合理

由于使用本质矩阵的分解,得到四组解,我们需要根据三角化,来判断四组解中比较合理的解。如下图所示,摘自《视觉SLAM十四讲》的内容。

image-20220630134015424

image-20220630112932295

/**
 * @brief 通过三角化来检查 R t是否合理
 * 
 * @param l l相机的观测
 * @param r r相机的观测
 * @param R 旋转矩阵
 * @param t 位移
 * @return double 
 */
double InitialEXRotation::testTriangulation(const vector<cv::Point2f> &l,
                                            const vector<cv::Point2f> &r,
                                            cv::Mat_<double> R, cv::Mat_<double> t)
{
    cv::Mat pointcloud;
    // 其中一帧设置为单位阵
    cv::Matx34f P = cv::Matx34f(1, 0, 0, 0,
                                0, 1, 0, 0,
                                0, 0, 1, 0);
    // 第二帧设置为R t 对应的位姿
    cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0),
                                 R(1, 0), R(1, 1), R(1, 2), t(1),
                                 R(2, 0), R(2, 1), R(2, 2), t(2));
    /**
     * @brief 进行三角化
     * @param  P  相机位姿
     * @param  P1 相机位姿
     * @param  l  特征点在相机位姿P下的坐标
     * @param  r  特征点在相机位姿P1下的坐标
     * @return pointcloud 三角化后的特征点的3D坐标
     */
    cv::triangulatePoints(P, P1, l, r, pointcloud);
    int front_count = 0;
    for (int i = 0; i < pointcloud.cols; i++)
    {
        // 这里为了后面求齐次坐标,拿出最后一维的值
        double normal_factor = pointcloud.col(i).at<float>(3);
      
        // 得到在各自坐标系下的3d坐标 
        cv::Mat_<double> p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor);
        cv::Mat_<double> p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor);
        // 通过深度是否大于0来判断是否合理
        if (p_3d_l(2) > 0 && p_3d_r(2) > 0)
            front_count++;
    }
    ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols);
    return 1.0 * front_count / pointcloud.cols;
}
🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/79.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 07 月 01 日
赏杯咖啡