(01)ORB-SLAM2源码无死角解析-(28) 双目Stereo相机立体匹配,SAD算法→深度求解

江南才尽，年少无知！

已于 2022-10-31 17:18:43 修改

阅读量1.6k

点赞数 3

分类专栏： # (01)ORB-SLAM2源码无死角解析-免费文章标签：自动驾驶机器人 ORB-SLAM2 增强现实无人机

于 2022-05-16 18:31:21 首次发布

原文链接：https://cvlife.net/detail/term_634260915e015_SerHfL/25?product_id=term_634260915e015_SerHfL

版权

(01)ORB-SLAM2源码无死角解析-免费专栏收录该内容

50 篇文章 108 订阅

订阅专栏

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的(01)ORB-SLAM2源码无死角解析链接如下(本文内容来自计算机视觉life ORB-SLAM2 课程课件):
(01)ORB-SLAM2源码无死角解析-(00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/123092196

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

该篇博客主要对双目相机的立体匹配进行一个讲解，也就是通过双目相机提供的信息求解深度。详细讲解之前，我们需要了解一下双目相机的基本属性，下面是双目相机的一个原理视图:
在这里插入图片描述
其上 Left 与 Right 分别表示两个相机的光心，他们的之间的距离也叫做基线 baseline (一般以米为单位)，从上图可以看到真实世界上的一个黑点，映射成图像坐标分别为 $(x, y)$ , $(x^{'}, y)$ ，那么其视差disparity为 $x^{'} - x$ (一般以像素为单位)。这里记 baseline为 $b$ ，disparity 为 $d$ 。

那么根据上图，可以明显的知道 $\frac{Z}{b}=\frac{f}{d}$ , 其中的 $f$ 表示相机焦距，该公式还是十分简单的，主要利用了一个等比性质，这里就不再进行详细的讲解了。该公式化简之后为 $Z=\frac{fb}{d}$ 。也就是说，知道一个特征点在两个相机成像位置之后，就可以求解视差 $d$ ，另外 $f$ , $b$ 一般为已知量，即可求解特征点对应的深度。

在这里插入图片描述
上图可以看到其最大视差的点为黄线与绿线的交点。因为这个3D点在左相机成像 $x$ 坐标是最大的，再右相机成像 $x^{'}$ 是最小的。此时他们具备最大视差。注意一个点，就是成像平面是关于 $(0, 0)$ 成像原点对称，也就是说单左成像面与右成像平面在3维空间接触的时，最大误差为 $x^{'} - x = b (ba se l in e)$ 。那么我们再来看看最小视察,如下图所示:
在这里插入图片描述

可以看到，当两条黄线平行的时候，可以很明显的知道此时的视差 $x^{'} - x = 0$ ，但是需要注意一个点，那就是此次两黄线是平行线，也就是 3维真实世界没有对应的点能真正的投影成 0 视差。可以想象得到，假设左下的黄线往右旋转无穷小，或者右上的黄线往左旋转无穷小，那么两条黄线将相较于无穷远处，也就是此时存在了交点。

从前面的第一张图像，可以看到，特征点成像 $(x, y)$ , $(x^{'}, y)$ 坐标，他们纵坐标是一样的，因为一般来说，双目相机都是固定的。简单的说，就是现实世界中的一点，投影到两个相机成像之后，连接两个成像点产生的直线，永远与基线 baseline 平行。换而言之，同一世界点成像坐标的 y 轴永远是相等的(建立在相机工艺完美的前提下)，如下图所示：
在这里插入图片描述
可以看到左图的一个特征点，对应于右图的可能存在的位置，其 y 轴坐标是相同的。也就是说，知道左图相机的一个特征代点，在进行特征匹配的时候，只需要跟右图 y 轴相同的像素进行匹配即可，二不需要匹配所有的像素点，这样就极度提高了特征匹配的效率。

二、源码概括

通过上面的讲解，已经大致明白了求解特征点深度的过程，其难点主要还在在于特征匹配。下面就来说说源码是如何进行特征匹配的。对应的源码为 src/Frame.cc 中的函数 void Frame::ComputeStereoMatches()

$\color{blue}{(1):}$ 行特征点统计. 统计img_right每一行上的ORB特征点集，便于使用立体匹配思路(行搜索/极线搜索）进行同名点搜索, 避免逐像素的判断.

$\color{blue}{(2):}$ 粗匹配. 根据步骤1的结果，对img_left第i行的orb特征点pi，在img_right的第i行上的orb特征点集中搜索相似orb特征点, 得到qi

$\color{blue}{(3):}$ 精确匹配. 以点qi为中心，半径为r的范围内，进行块匹配（归一化SAD），进一步优化匹配结果

$\color{blue}{(4):}$ 亚像素精度优化. 步骤3得到的视差为uchar/int类型精度，并不一定是真实视差，通过亚像素差值（抛物线插值)获取float精度的真实视差

$\color{blue}{(5):}$ 最优视差值/深度选择. 通过胜者为王算法（WTA）获取最佳匹配点。

$\color{blue}{(6):}$ 删除离群点(outliers). 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是正确匹配，比如光照变化、弱纹理等会造成误匹配

根据上面的总结，下面再对其上上的难点进行细致的分析，主要对源码进行细致的讲解(后面有较全的源码注释，大家可以参考)

$\color{blue}{(1):}$ 首先统计按行统计右图特征点坐标位置，存储于vRowIndices 变量之中，比如 vRowIndices[i], 其中 i 表示行数(行坐标)，mvKeysRight[i] 为一个数组，数组中的值，即为多个特征点的列数(列坐标)。但是这样还不够的，为了减少误差(由于双目相机制造工艺)，需要为 vRowIndices 的每一行，额外添加一些来自其他行特征点的列坐标。代码中由变量 const float r = 2.0f * mvScaleFactors[mvKeysRight[iR].octave]; 决定了偏移范围。

$\color{blue}{(2):}$ 计算出最小视差与最大视差，因为在左图中的特征点，可能在右图是看不见的。也就是说，前面根据双目成像原理，已经把特征匹配缩小至右图的一行所有特征点(加上偏移范围的特征点)。另外，进一步根据视察的原理，继续缩小匹配范围。然后再进行特征匹配，找到最好的匹配点

$\color{blue}{(3):}$ 通过前面两点，进行了粗匹配，那么接下来还要进行一个精细匹配。分别以右图特征点，以上面第(2)获得最好的特征点为中心，分别提取半径为w的图像块patch，然后利用滑窗口匹配→左边覆盖区域减去右边覆盖区域，并求出所有像素点差的绝对值的和(SAD值)。找到这个范围内SAD值最小的窗口，即找到了左边图像的最佳匹配的像素块。

$\color{blue}{(4):}$ 根据(3)，已经计算出了左右图像块patch中相似度最高的位置。但是如果该位置两边的相似度差值较大，则认为是误匹配，否则进行修复。比如 dist2 表示最高相似度，dist1，dist3表示其旁边的现实度，那么通过公式 deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2))，可以计算出需要修复的量，如果修复量太大了，说明 dist1，dist3 与 dist2之间的差异较大，则认为是误匹配，具体的可以参考 opencv sgbm源码中的亚像素插值公式，或者论文 <> 公式7。

$\color{blue}{(5):}$ 根据左右图匹配到的特征点位置，计算视差，进一步求解深度。

$\color{blue}{(6):}$ 删除离群点(outliers). 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是正确匹配，比如光照变化、弱纹理等会造成误匹配

三、源码注解

src/Frame.cc 文件中的 void Frame::ComputeStereoMatches() 函数:

/*
 * 双目匹配函数
 *
 * 为左图的每一个特征点在右图中找到匹配点 \n
 * 根据基线(有冗余范围)上描述子距离找到匹配, 再进行SAD精确定位 \n ‘
 * 这里所说的SAD是一种双目立体视觉匹配算法，可参考[https://blog.csdn.net/u012507022/article/details/51446891]
 * 最后对所有SAD的值进行排序, 剔除SAD值较大的匹配对，然后利用抛物线拟合得到亚像素精度的匹配 \n 
 * 这里所谓的亚像素精度，就是使用这个拟合得到一个小于一个单位像素的修正量，这样可以取得更好的估计结果，计算出来的点的深度也就越准确
 * 匹配成功后会更新 mvuRight(ur) 和 mvDepth(Z)
 */
void Frame::ComputeStereoMatches()
{
    /*两帧图像稀疏立体匹配（即：ORB特征点匹配，非逐像素的密集匹配，但依然满足行对齐）
     * 输入：两帧立体矫正后的图像img_left 和 img_right 对应的orb特征点集
     * 过程：
          1. 行特征点统计. 统计img_right每一行上的ORB特征点集，便于使用立体匹配思路(行搜索/极线搜索）进行同名点搜索, 避免逐像素的判断.
          2. 粗匹配. 根据步骤1的结果，对img_left第i行的orb特征点pi，在img_right的第i行上的orb特征点集中搜索相似orb特征点, 得到qi
          3. 精确匹配. 以点qi为中心，半径为r的范围内，进行块匹配（归一化SAD），进一步优化匹配结果
          4. 亚像素精度优化. 步骤3得到的视差为uchar/int类型精度，并不一定是真实视差，通过亚像素差值（抛物线插值)获取float精度的真实视差
          5. 最优视差值/深度选择. 通过胜者为王算法（WTA）获取最佳匹配点。
          6. 删除离群点(outliers). 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是正确匹配，比如光照变化、弱纹理等会造成误匹配
     * 输出：稀疏特征点视差图/深度图（亚像素精度）mvDepth 匹配结果 mvuRight
     */

    // 为匹配结果预先分配内存，数据类型为float型
    // mvuRight存储右图匹配点索引
    // mvDepth存储特征点的深度信息
	mvuRight = vector<float>(N,-1.0f);
    mvDepth = vector<float>(N,-1.0f);

	// orb特征相似度阈值  -> mean ～= (max  + min) / 2
    const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;

    // 金字塔底层（0层）图像高 nRows
    const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;

	// 二维vector存储每一行的orb特征点的列坐标，为什么是vector，因为每一行的特征点有可能不一样，例如
    // vRowIndices[0] = [1，2，5，8, 11]   第1行有5个特征点,他们的列号（即x坐标）分别是1,2,5,8,11
    // vRowIndices[1] = [2，6，7，9, 13, 17, 20]  第2行有7个特征点.etc
    vector<vector<size_t> > vRowIndices(nRows, vector<size_t>());
    for(int i=0; i<nRows; i++) vRowIndices[i].reserve(200);

	// 右图特征点数量，N表示数量 r表示右图，且不能被修改
    const int Nr = mvKeysRight.size();

	// Step 1. 行特征点统计。 考虑用图像金字塔尺度作为偏移，左图中对应右图的一个特征点可能存在于多行，而非唯一的一行
    for(int iR = 0; iR < Nr; iR++) {

        // 获取特征点ir的y坐标，即行号
        const cv::KeyPoint &kp = mvKeysRight[iR];
        const float &kpY = kp.pt.y;
        
        // 计算特征点ir在行方向上，可能的偏移范围r，即可能的行号为[kpY + r, kpY -r]
        // 2 表示在全尺寸(scale = 1)的情况下，假设有2个像素的偏移，随着尺度变化，r也跟着变化
        const float r = 2.0f * mvScaleFactors[mvKeysRight[iR].octave];
        const int maxr = ceil(kpY + r);
        const int minr = floor(kpY - r);

        // 将特征点ir保证在可能的行号中
        for(int yi=minr;yi<=maxr;yi++)
            vRowIndices[yi].push_back(iR);
    }

    // 下面是 粗匹配 + 精匹配的过程
    // 对于立体矫正后的两张图，在列方向(x)存在最大视差maxd和最小视差mind
    // 也即是左图中任何一点p，在右图上的匹配点的范围为应该是[p - maxd, p - mind], 而不需要遍历每一行所有的像素
    // maxd = baseline * length_focal / minZ
    // mind = baseline * length_focal / maxZ

    const float minZ = mb;
    const float minD = 0;			// 最小视差为0，对应无穷远 
    const float maxD = mbf/minZ;    // 最大视差对应的距离是相机的基线

    // 保存sad块匹配相似度和左图特征点索引
    vector<pair<int, int> > vDistIdx;
    vDistIdx.reserve(N);

    // 为左图每一个特征点il，在右图搜索最相似的特征点ir
    for(int iL=0; iL<N; iL++) {

		const cv::KeyPoint &kpL = mvKeys[iL];
        const int &levelL = kpL.octave;
        const float &vL = kpL.pt.y;
        const float &uL = kpL.pt.x;

        // 获取左图特征点il所在行，以及在右图对应行中可能的匹配点
        const vector<size_t> &vCandidates = vRowIndices[vL];
        if(vCandidates.empty()) continue;

        // 计算理论上的最佳搜索范围
        const float minU = uL-maxD;
        const float maxU = uL-minD;
        
        // 最大搜索范围小于0，说明无匹配点
        if(maxU<0) continue;

		// 初始化最佳相似度，用最大相似度，以及最佳匹配点索引
        int bestDist = ORBmatcher::TH_HIGH;
        size_t bestIdxR = 0;
        const cv::Mat &dL = mDescriptors.row(iL);
        
        // Step 2. 粗配准。左图特征点il与右图中的可能的匹配点进行逐个比较,得到最相似匹配点的描述子距离和索引
        for(size_t iC=0; iC<vCandidates.size(); iC++) {

            const size_t iR = vCandidates[iC];
            const cv::KeyPoint &kpR = mvKeysRight[iR];

            // 左图特征点il与待匹配点ic的空间尺度差超过2，放弃
            if(kpR.octave<levelL-1 || kpR.octave>levelL+1)
                continue;

            // 使用列坐标(x)进行匹配，和stereomatch一样
            const float &uR = kpR.pt.x;

            // 超出理论搜索范围[minU, maxU]，可能是误匹配，放弃
            if(uR >= minU && uR <= maxU) {

                // 计算匹配点il和待匹配点ic的相似度dist
                const cv::Mat &dR = mDescriptorsRight.row(iR);
                const int dist = ORBmatcher::DescriptorDistance(dL,dR);

				//统计最小相似度及其对应的列坐标(x)
                if( dist<bestDist ) {
                    bestDist = dist;
                    bestIdxR = iR;
                }
            }
        }
    
        // Step 3. 图像块滑动窗口用SAD(Sum of absolute differences，差的绝对和)实现精确匹配. 
        if(bestDist<thOrbDist) {
            // 如果刚才匹配过程中的最佳描述子距离小于给定的阈值
            // 计算右图特征点x坐标和对应的金字塔尺度
            const float uR0 = mvKeysRight[bestIdxR].pt.x;
            const float scaleFactor = mvInvScaleFactors[kpL.octave];
            
            // 尺度缩放后的左右图特征点坐标
            const float scaleduL = round(kpL.pt.x*scaleFactor);			
            const float scaledvL = round(kpL.pt.y*scaleFactor);
            const float scaleduR0 = round(uR0*scaleFactor);

            // 滑动窗口搜索, 类似模版卷积或滤波
            // w表示sad相似度的窗口半径
            const int w = 5;

            // 提取左图中，以特征点(scaleduL,scaledvL)为中心, 半径为w的图像块patch
            cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);
            IL.convertTo(IL,CV_32F);
            
            // 图像块均值归一化，降低亮度变化对相似度计算的影响
			IL = IL - IL.at<float>(w,w) * cv::Mat::ones(IL.rows,IL.cols,CV_32F);

			//初始化最佳相似度
            int bestDist = INT_MAX;

			// 通过滑动窗口搜索优化，得到的列坐标偏移量
            int bestincR = 0;

			//滑动窗口的滑动范围为（-L, L）
            const int L = 5;

			// 初始化存储图像块相似度
            vector<float> vDists;
            vDists.resize(2*L+1); 

            // 计算滑动窗口滑动范围的边界，因为是块匹配，还要算上图像块的尺寸
            // 列方向起点 iniu = r0 - 最大窗口滑动范围 - 图像块尺寸
            // 列方向终点 eniu = r0 + 最大窗口滑动范围 + 图像块尺寸 + 1
            // 此次 + 1 和下面的提取图像块是列坐标+1是一样的，保证提取的图像块的宽是2 * w + 1
            // ! 源码： const float iniu = scaleduR0+L-w; 错误
            // scaleduR0：右图特征点x坐标
            const float iniu = scaleduR0-L-w;
            const float endu = scaleduR0+L+w+1;

			// 判断搜索是否越界
            if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols)
                continue;

			// 在搜索范围内从左到右滑动，并计算图像块相似度
            for(int incR=-L; incR<=+L; incR++) {

                // 提取右图中，以特征点(scaleduL,scaledvL)为中心, 半径为w的图像快patch
                cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1);
                IR.convertTo(IR,CV_32F);
                
                // 图像块均值归一化，降低亮度变化对相似度计算的影响
                IR = IR - IR.at<float>(w,w) * cv::Mat::ones(IR.rows,IR.cols,CV_32F);
                
                // sad 计算，值越小越相似
                float dist = cv::norm(IL,IR,cv::NORM_L1);

                // 统计最小sad和偏移量
                if(dist<bestDist) {
                    bestDist = dist;
                    bestincR = incR;
                }

                //L+incR 为refine后的匹配点列坐标(x)
                vDists[L+incR] = dist; 	
            }

            // 搜索窗口越界判断
            if(bestincR==-L || bestincR==L)
                continue;

			// Step 4. 亚像素插值, 使用最佳匹配点及其左右相邻点构成抛物线来得到最小sad的亚像素坐标
            // 使用3点拟合抛物线的方式，用极小值代替之前计算的最优是差值
            //    \                 / <- 由视差为14，15，16的相似度拟合的抛物线
            //      .             .(16)
            //         .14     .(15) <- int/uchar最佳视差值
            //              . 
            //           （14.5）<- 真实的视差值
            //   deltaR = 15.5 - 16 = -0.5
            // 公式参考opencv sgbm源码中的亚像素插值公式
            // 或论文<<On Building an Accurate Stereo Matching System on Graphics Hardware>> 公式7

            const float dist1 = vDists[L+bestincR-1];	
            const float dist2 = vDists[L+bestincR];
            const float dist3 = vDists[L+bestincR+1];
            const float deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2));

            // 亚像素精度的修正量应该是在[-1,1]之间，否则就是误匹配
            if(deltaR<-1 || deltaR>1)
                continue;
            
            // 根据亚像素精度偏移量delta调整最佳匹配索引
            float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);
            float disparity = (uL-bestuR);
            if(disparity>=minD && disparity<maxD) {
                // 如果存在负视差，则约束为0.01
                if( disparity <=0 ) {
                    disparity=0.01;
                    bestuR = uL-0.01;
                }
                
                // 根据视差值计算深度信息
                // 保存最相似点的列坐标(x)信息
                // 保存归一化sad最小相似度
                // Step 5. 最优视差值/深度选择.
                mvDepth[iL]=mbf/disparity;
                mvuRight[iL] = bestuR;
                vDistIdx.push_back(pair<int,int>(bestDist,iL));
        }   
    }
    }
    // Step 6. 删除离群点(outliers)
    // 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是匹配的，比如光照变化、弱纹理、无纹理等同样会造成误匹配
    // 误匹配判断条件  norm_sad > 1.5 * 1.4 * median
    sort(vDistIdx.begin(),vDistIdx.end());
    const float median = vDistIdx[vDistIdx.size()/2].first;
    const float thDist = 1.5f*1.4f*median;

    for(int i=vDistIdx.size()-1;i>=0;i--) {
        if(vDistIdx[i].first<thDist)
            break;
        else {
			// 误匹配点置为-1，和初始化时保持一直，作为error code
            mvuRight[vDistIdx[i].second]=-1;
            mvDepth[vDistIdx[i].second]=-1;
        }
    }
}

四、结语

通过该篇博客的介绍，我们已经知道如何通过双目相机去求解特征代点的深度。同时了解了 SAD 以及亚像素插值算法。

本文内容来自计算机视觉life ORB-SLAM2 课程课件

江南才尽，年少无知！

关注

3
点赞
踩
14

收藏

觉得还不错? 一键收藏
0
评论
(01)ORB-SLAM2源码无死角解析-(28) 双目Stereo相机立体匹配,SAD算法→深度求解

该篇博客主要对双目相机的立体匹配进行一个讲解，也就是通过双目相机提供的信息求解深度。详细讲解之前，我们需要了解一下双目相机的基本属性，下面是双目相机的一个原理视图:其上 Left 与 Right 分别表示两个相机的光心，他们的之间的距离也叫做基线 baseline (一般以米为单位)，从上图可以看到真实世界上的一个黑点，映射成图像坐标分别为(x,y)(x,y)(x,y),(x′,y)(x',y)(x′,y)，那么其视差disparity为x′−x。
复制链接

扫一扫