VINS-Mono+Fusion源码解析系列（十三）：纯视觉SFM

最新推荐文章于 2024-12-20 17:39:02 发布

原创

最新推荐文章于 2024-12-20 17:39:02 发布 · 982 阅读

标签

#计算机视觉 #人工智能 #自动驾驶

本文详细解析了VINS-Mono融合中纯视觉SFM的实现，包括选择枢纽帧、计算相对位姿、滑窗中的SFM过程，涉及三角化、PnP求解等关键技术。通过滑窗中每一帧的位姿和3D共视点的求解，最终通过BA优化提高精度。

1. 视觉SFM理论

假设滑窗中一共有11帧，首先需要选取一个枢纽帧，利用枢纽帧和最后一帧通过对极约束求出这两帧之间的相对位姿。对枢纽帧的要求是：一方面要求枢纽帧离最后一帧尽可能远，因为离最后一帧比较近时，二者之间的平移比较小，使得三角化精度较差，甚至会无法进行对极约束（平移为零的情况）。另一方面，要求枢纽帧与最后一帧之间不能距离太远，否则会使得二者之间关联的特征点比较少，导致求解出的位姿可信度不高，并且无法三角化出尽可能多的点。因此，在保证匹配点足够多的情况下，枢纽帧离最后一帧应该尽可能远，如下图中选取第4帧作为枢纽帧。

将枢纽帧设为参考帧，其位姿设置：旋转为单位阵 $I$ ，平移为0。通过对极几何求出枢纽阵(4)和最后一帧(10)之间的相对位姿R和t（尺度未知）。由于第4帧的位姿为 $(I, 0)$ ，那么第10帧的位姿就是二者之间的相对位姿 $(R, t)$ 。然后，在已知位姿之后，通过三角化求出这两帧之间的共视点（带尺度的）。由于光流追踪的性质，求出的第4帧与第10帧之间的共视点也会被中间的第5—9帧所看到，那么在已知这些共视点（3D）和第4帧的关键点（2D）之后，通过PnP的方法（3D-2D）求解出第5帧的位姿。按照同样的方法，通过三角化获取更多的共视点，通过PnP求出枢纽帧(4)和最后一帧(10)之间的每一帧(5—9)的位姿。

枢纽帧(4)与第一帧(0)之间的位姿和共视点求取按照同样的方法，先根据对极几何求出第0帧和第4帧的相对位姿，由于第4帧的位姿为 $(I, 0)$ ，那么求出的相对位姿就是第0帧的位姿。然后通过三角化求出第0帧与第4帧之间的共视点，根据光流追踪的性质，这些点都会被第1—3帧所看到。然后通过PnP的方法求出第1帧的位姿，以此类推，通过三角化获取更多的共视点，通过PnP求出第2、3帧的位姿。

通过对滑窗进行上述操作，可求出滑窗中每一帧的位姿和所看到的3D共视点。
视觉SFM

2. 获取枢纽帧并计算相对位姿relativePose

（1）大致流程

遍历滑窗中的每一帧，通过f_manager获取遍历到的当前帧与最后一帧之间的匹配对
通过f_manager直接获取这一匹配对的共视点个数，在共视点个数超过20个的前提下，执行如下操作：
- 获取每个共视点在各自帧中的像素坐标，根据像素坐标计算出其视差，进而计算出所有共视点的平均视差
- 结合虚拟相机焦距（460像素/mm），满足平均视差超过30个像素（这样保证枢纽帧与最后一帧具有足够的距离）的当前帧作为枢纽帧
- 然后，通过对极约束，调用solveRelativeRT计算出枢纽帧与滑窗中最后一帧之间的相对位姿

（2）代码实现

/**
 * @brief 寻找滑窗内一个帧作为枢纽帧，要求和最后一帧既有足够的共视也要有足够的视差
 *        并求出枢纽帧与滑窗中最后一帧之间的相对位姿
 *
 * @param[in] relative_R 
 * @param[in] relative_T 
 * @param[in] l 
 * @return true 
 * @return false 
 */

bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
   
   
    // 优先从最前面开始遍历，确定哪一帧能够选定为枢纽帧
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
   
   
        vector<pair<Vector3d, Vector3d>> corres;
        // corres表示第i帧与最后一帧WINDOW_SIZE同时观测到的3D点在各自归一化坐标系下的投影点
        corres = f_manager.getCorresponding(i, WINDOW_SIZE); 
        // 要求共视的特征点足够多
        if (corres.size() > 20)  // 若第i帧与最后一帧的共视点个数 大于 20
        {
   
   
            double sum_parallax = 0;
            double average_parallax;
            for (int j = 0; j < int(corres.size()); j++)
            {
   
    
                // 第j个共视点在当前第i帧中的像素坐标(u, v)
                Vector2d pts_0(corres[j].first(0), corres[j].first(1));   
                // 第j个共视点在最后一帧中的像素坐标(u, v)
                Vector2d pts_1(corres[j].second(0), corres[j].second(1));  
                double parallax = (pts_0 - pts_1).norm();   // 计算二者之间的视差
                sum_parallax = sum_parallax + parallax;     // 求所有共视点的视差和

            }
            // 计算两帧之间共视点的平均视差
            average_parallax = 1.0 * sum_parallax / int(corres.size());
/*
   要求平均视差在虚拟相机的焦距(460像素/mm)中 大于 30，就认为当前帧i与最后一帧之间具有足够的视差
   不同相机的视差判断阈值就不一样，这里使用虚拟相机焦距，使得视差阈值统一为30个像素
   然后，利用对极约束，通过本质矩阵恢复枢纽帧和最后一帧之间的相对位姿 R t ————  solveRelativeRT
*/
            if(average_parallax * 460 > 30 && 
               				m_estimator.solveRelativeRT(corres, relative_R, relative_T))
            {
   
   
              // 枢纽帧: 既与最后一帧有足够的视差，又能够使用对极约束成功计算出与最后一帧之间的相对位姿
                l = i; 
                return true;
            }
        }
    }
    return false;
}

（3）对极约束求解相对位姿

大致流程

首先，在共视特征点匹配对个数不少于15的前提下，获取输入的两关键帧的共视特征点匹配对
根据两关键帧上的匹配像素点，调用opencv函数cv::findFundamentalMat计算本质矩阵E
调用opencv函数cv::recoverPose，根据本质矩阵恢复出位姿R和t，并返回内点个数（用于衡量匹配点中满足求解出的R和t的点的个数）
求解出的位姿是cv::Mat格式的R21和t21，先将其转换成eigen格式，然后通过下面推导出的公式，将其转换成R12和t12。并且，只有当内点个数超过12时，求解出的R12和t12才有效。

假设第一帧的位姿为单位矩阵，要求第二帧位姿，则相对位姿 $R_{21}$ 和 $t_{21}$ 就是第二帧的位姿。二者之间的转换关系推导如下：
$P_1 = R_{12} \cdot P_2 + t_{12} \ \ \ \ \ \ \ \ \ \ P_2 = R_{21} \cdot P_1 + t_{21} \\ \Rightarrow \\ R_{12}^T \cdot P_1 = P_2 + R_{12}^T \cdot t_{12} \ \ \ \ \ P_2 = R_{12}^T \cdot P_1 - R_{12}^T\cdot t_{12} \\ \Rightarrow \\ R_{21} = R_{12}^T\ \ \ \ \ t_{21} = -R_{12}^T \cdot t_{12} \\ \Rightarrow \\ R_{12} = R_{21}^T\ \ \ \ \ t_{12} = -R_{21}^T \cdot t_{21}$
代码实现

/**
 * @brief 根据两帧匹配对求解R和带尺度的t
 * 
 * @param[in] corres        两帧的匹配对
 * @param[out] Rotation     求解出的相对旋转
 * @param[out] Translation  求解出的相对平移
 * @return true 
 * @return false 
 */

bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
   
   
    if (corres.size() >= 15)  // 两帧的匹配点对至少要有15个
    {
   
   
        vector<cv::Point2f> ll, rr;  // 获取匹配点对的坐标
        for (int i = 0; i < int(corres.size()); i++)
        {
   
   
            ll.push_back(cv::

最低0.47元/天解锁文章