ORB-SLAM3算法学习—Frame构造—基于SAD滑窗的双目特征匹配

0总述

简单的说，ORB-SLAM中的双目匹配只对特征点进行操作，根据左目图像中的特征点坐标搜索其在右目图像的对应匹配点，并将右目图像匹配点的x坐标储存在成员变量mvuRight中，有了特征点在右目图像的坐标就可以计算视差disparity进而计算深度距离信息，最终深度信息保存在成员变量mvDepth。
具体的几何原理图如下：

1双目匹配

1.1为左目每个特征点建立带状区域搜索表，限定搜索区域。（已提前极线校正）

首先为双目匹配初始化一个带状搜索表vRowIndices,这是一个二维向量，外层大小为图像的行数nRows，记录特征点在右目图像的纵坐标，内曾大小为带状区域的宽度，记录特征点的索引。

vector<vector<size_t> > vRowIndices(nRows,vector<size_t>());

在右目图像为左目特征点进行匹配搜索的时候，不仅仅是在一条横线上搜索，而是在一条横向搜索带上搜索。简而言之，原本每个特征点的纵坐标为1个像素大小，这里把特征点体积放大，假定纵坐标占好几行

例如左目图像某个特征点的纵坐标为20，那么在右侧图像上搜索时是在纵坐标为18到22这条带上搜索，搜索带宽度为正负2，搜索带的宽度和特征点所在金字塔层数有关
简单来说，如果纵坐标是20，特征点在左目图像第20行，那么认为右目图像18 19 20 21 22行都有这个特征点

代码片段

    const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;// 左目图像，金字塔第一层即原始图像的行数，即高度
    // Assign keypoints to row table
    // 步骤1：建立特征点搜索范围对应表，一个特征点在一个带状区域内搜索匹配特征点
    // 匹配搜索的时候，不仅仅是在一条横线上搜索，而是在一条横向搜索带上搜索,简而言之，原本每个特征点的纵坐标为1，这里把特征点体积放大，纵坐标占好几行
    // 例如左目图像某个特征点的纵坐标为20，那么在右侧图像上搜索时是在纵坐标为18到22这条带上搜索，搜索带宽度为正负2，搜索带的宽度和特征点所在金字塔层数有关
    // 简单来说，如果纵坐标是20，特征点在图像第20行，那么认为18 19 20 21 22行都有这个特征点
    // vRowIndices[18]、vRowIndices[19]、vRowIndices[20]、vRowIndices[21]、vRowIndices[22]都有这个特征点编号
    vector<vector<size_t> > vRowIndices(nRows,vector<size_t>());
    for(int i=0; i<nRows; i++)
        vRowIndices[i].reserve(200);
    const int Nr = mvKeysRight.size();// 右目特征点的数量
    // 将右目特征点的索引和带状区域关联
    for(int iR=0; iR<Nr; iR++)
    {
        // !!在这个函数中没有对双目进行校正，双目校正是在外层程序中实现的
        const cv::KeyPoint &kp = mvKeysRight[iR];
        const float &kpY = kp.pt.y;
        // 计算匹配搜索的纵向宽度，尺度越大（层数越高，距离越近），搜索范围越大
        // 如果特征点在金字塔第一层，则搜索范围为:正负2
        // 尺度越大其位置不确定性越高，所以其搜索半径越大
        const float r = 2.0f*mvScaleFactors[mvKeysRight[iR].octave];
        const int maxr = ceil(kpY+r);// ceil向上取整函数
        const int minr = floor(kpY-r);// floor向下取整函数
        // 将索引和带状区域关联起来
        for(int yi=minr;yi<=maxr;yi++)
            vRowIndices[yi].push_back(iR);
    }

1.2对左目相机每个特征点，通过描述子在右目带状搜索区域找到匹配点

首先，根据左目特征点的纵坐标y，找到右目图像对应带状区域里所有右目候选匹配特征点的索引

const vector<size_t> &vCandidates = vRowIndices[vL];

然后，遍历右目所有选匹配特征点，分别与左目的该特征点计算描述子距离，记录描述子距离最小对应的右目特征点id

const int dist = ORBmatcher::DescriptorDistance(dL,dR);

对应代码片段

        const cv::KeyPoint &kpL = mvKeys[iL];// 取出一个左目的特征点
        const int &levelL = kpL.octave;// 特征点的尺度
        const float &vL = kpL.pt.y;// 特征点纵坐标
        const float &uL = kpL.pt.x;// 特征点横坐标
        // 右目图像中可能的候选匹配点
        // 根据特征点的纵坐标，快速找到右目对应带状区域的里所有右目特征点的索引
        const vector<size_t> &vCandidates = vRowIndices[vL];
        // 找不到就认为该特征点没有右目的匹配点
        if(vCandidates.empty())
            continue;

        // 根据相机允许的最大最小视差，确定一个x轴方向上的范围
        const float minU = uL-maxD;// 最小匹配范围
        const float maxU = uL-minD;// 最大匹配范围
        if(maxU<0)// minD=0,maxU<0说明uL<0，是无效点
            continue;

        int bestDist = ORBmatcher::TH_HIGH;// 初始化最佳匹配距离，会不断更新
        size_t bestIdxR = 0;// 最佳匹配点对应的id
        // 取出该左目特征点对应的描述子，每个特征点描述子占一行，建立一个指针指向iL特征点对应的描述子
        const cv::Mat &dL = mDescriptors.row(iL);

        // Compare descriptor to right keypoints
        // 步骤2.1：遍历右目所有可能的匹配点，找出最佳匹配点（描述子距离最小）
        for(size_t iC=0; iC<vCandidates.size(); iC++)
        {
            const size_t iR = vCandidates[iC];// 右目候选特征点索引
            const cv::KeyPoint &kpR = mvKeysRight[iR];
            // 仅对近邻尺度的特征点进行匹配
            if(kpR.octave<levelL-1 || kpR.octave>levelL+1)
                continue;

            const float &uR = kpR.pt.x;// 右目候选匹配点的x坐标
            // 要确保右目候选匹配点的坐标也在合理的视差范围内
            if(uR>=minU && uR<=maxU)
            {
                const cv::Mat &dR = mDescriptorsRight.row(iR);// 取出右目候选匹配点对应的描述子
                const int dist = ORBmatcher::DescriptorDistance(dL,dR);// 计算左右目匹配点的描述子距离

                // 更新最小匹配距离和其对应的右目特征点索引
                if(dist<bestDist)
                {
                    bestDist = dist;
                    bestIdxR = iR;
                }
            }

1.3通过SAD滑窗得到匹配修正量bestincR

首先，将上面的到的匹配对的坐标乘以一个尺度因子，变成对应金字塔层的坐标，scaleduL，scaledvL，scaleduR0分别是左目特征点x坐标，左目特征点y坐标和右目特征点x坐标。

然后，从左目特征点所在金字塔层的图像中取出一个图像块，该图像块以特征点为中心，取11x11个像素区域

然后，在右目图像中进行滑窗框选同样大小的像素块，计算两个像素块所有像素灰度值之差的绝对值之和，在滑窗移动的过程中不断得更新最小差值bestDist，以及差值最小时对应的修正量bestincR。这个修正量bestincR是说，以最初匹配到的右目特征点x坐标scaleduR0为基准，当移动bestincR后得到的新坐标(scaleduR0+bestincR)和左目特征点周围的像素信息差异更小，也就更加匹配。

这样就会得到一个抛物线，因为如果真正存在一个最佳修正量的话，越接近该位置像素灰度值差异就会越小，匹配偏差就越小;越远离该位置差异就越大，匹配偏差就越大
如果极小值出现的位置在两个边界出，说明没有出现拐点，即没有找到最小值，放弃计算该对匹配点的深度

注意的是，这里的最佳修正量bestincR并一定不是亚像素级别(可以简单的理解为像素坐标精确到小数点后 )的，因为在进行滑窗遍历的时候，步长是1,这就导致(scaleduR0+bestincR)处不一定是抛物线的谷底处，这就有了下面一步进行抛物线拟合。

代码片段

    // kpL.pt.x对应金字塔最底层坐标，将最佳匹配的特征点对的xy坐标使用尺度变换到尺度对应层 (scaleduL, scaledvL) (scaleduR0, )
    const float uR0 = mvKeysRight[bestIdxR].pt.x;// 右目图像特征点在金字塔底层的x坐标
    const float scaleFactor = mvInvScaleFactors[kpL.octave];// 该左目特征点的尺度
    const float scaleduL = round(kpL.pt.x*scaleFactor);// 左目x坐标
    const float scaledvL = round(kpL.pt.y*scaleFactor);// 右目y坐标
    const float scaleduR0 = round(uR0*scaleFactor); // 右目x坐标

    // sliding window search
    const int w = 5;// 滑动窗口的大小11*11 注意该窗口取自resize后的图像
    // 从左目特征点所在金字塔层的图像中取出一个图像块，该图像块以特征点为中心，取11*11个像素区域
    cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);

    int bestDist = INT_MAX;// 重置最佳匹配距离
    int bestincR = 0;// 最佳坐标修正量 
    const int L = 5;
    vector<float> vDists;
    vDists.resize(2*L+1);
    // 滑动窗口的滑动范围为（-L, L）,提前判断滑动窗口滑动过程中是否会越界
    // iniu和endu为窗口的左右起点和终点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++)
    {
        // 横向滑动窗口
        // 这里L和w的值相等，所以遍历的范围是以scaleduR0为中心，左边界为scaleduR0-L-w,右边界为scaleduR0+L+w+1
        cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1);

        float dist = cv::norm(IL,IR,cv::NORM_L1);// 一范数，计算差的绝对值
        if(dist<bestDist)
        {
            bestDist =  dist;// SAD匹配目前最小匹配偏差
            bestincR = incR;// SAD匹配目前最佳的修正量
        }
        // 正常情况下，这里面的数据应该以抛物线形式变化
        // 因为如果真正存在一个最佳修正量的话，越接近该位置像素灰度值差异就会越小，匹配偏差就越小;越远离该位置差异就越大，匹配偏差就越大
        vDists[L+incR] = dist;
    }

    // 整个滑动窗口过程中，SAD最小值不是以抛物线形式出现，说明没有出现极小值，SAD匹配失败，同时放弃求该特征点的深度
    if(bestincR==-L || bestincR==L)
        continue;

1.4 做抛物线拟合找谷底得到亚像素匹配deltaR

上面我们已经证明了，存在这么一个抛物线，它的谷底(匹配误差最小)处对应的x坐标就是我们要找的精确的匹配坐标。
尽管在最佳修正量L+bestincR处不一定得到最小值，但最小值一定在最佳修正量的附近，因此可以通过(L+bestincR,dist1)、(L+bestincR-1,dist2)、(L+bestincR+1,dist3)三个点拟合出抛物线，做抛物线拟合找抛物线谷底得到亚像素修正量deltaR，deltaR是在L+bestincR的基础上更细微的一个变化量，因此最终的匹配点坐标bestuR为：

bestuR = scaleduR0+bestincR+deltaR

在计算出右目特征点准确的x坐标后，就可以计算视差了，然后进一步可以计算出这对匹配点对应的深度距离信息。深度信息计算就比较简单了，就是用配置文件的mbf除以视差disparity

截至到这一步，我们已经得到左目所有的特征点在右目图像对应特征点的坐标和对应的深度信息。代码中又进行了一步筛选，对于通过SAD滑窗计算出的匹配偏差较大的特征点对应的深度值设置为-1。
代码片段

    // 步骤2.3：做抛物线拟合找谷底得到亚像素匹配deltaR
    // (L+bestincR,dist) (L+bestincR-1,dist) (L+bestincR+1,dist)三个点拟合出抛物线
    // bestincR+deltaR就是抛物线谷底的位置，相对SAD匹配出的最小值bestincR的修正量为deltaR
    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));
    // 抛物线拟合得到的修正量不能超过一个像素，否则放弃求该特征点的深度
    if(deltaR<-1 || deltaR>1)
        continue;

    // Re-scaled coordinate
    // 通过描述子匹配得到匹配点位置为scaleduR0
    // 通过SAD匹配找到修正量bestincR
    // 通过抛物线拟合找到亚像素修正量deltaR
    float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);
    // 这里是disparity，根据它算出depth
    float disparity = (uL-bestuR);

    if(disparity>=minD && disparity<maxD)// 最后判断视差是否在范围内
    {
        if(disparity<=0)
        {
            disparity=0.01;
            bestuR = uL-0.01;
        }
        // depth 是在这里计算的
        // depth=baseline*fx/disparity
        mvDepth[iL]=mbf/disparity;// 深度
        mvuRight[iL] = bestuR;// 匹配对在右图的横坐标
        vDistIdx.push_back(pair<int,int>(bestDist,iL));// 该特征点SAD匹配最小匹配偏差
    }

    // 步骤3：剔除SAD匹配偏差较大的匹配特征点
    // 前面SAD匹配只判断滑动窗口中是否有局部最小值，这里通过对比剔除SAD匹配偏差比较大的特征点的深度
    sort(vDistIdx.begin(),vDistIdx.end());
    const float median = vDistIdx[vDistIdx.size()/2].first;
    const float thDist = 1.5f*1.4f*median;
    //lusx count
    int count_depth = vDistIdx.size();
    for(int i=vDistIdx.size()-1;i>=0;i--)
    {
        if(vDistIdx[i].first<thDist)
            break;
        else
        {
            count_depth--;
            mvuRight[vDistIdx[i].second]=-1;
            mvDepth[vDistIdx[i].second]=-1;
        }
    }

2 双目立体视觉匹配中的SAD滑窗算法

2.1算法原理

SAD(Sum of absolute differences)是一种图像匹配算法。基本思想：差的绝对值之和。此算法常用于图像块匹配，将每个像素对应数值之差的绝对值求和，据此评估两个图像块的相似度。该算法快速、但并不精确，通常用于多级处理的初步筛选。

2.2算法基本流程

1. 构造一个小窗口,类似于卷积核；
2. 用窗口覆盖左边的图像,选择出窗口覆盖区域内的所有像素点；
3. 同样用窗口覆盖右边的图像并选择出覆盖区域的像素点；
4. 左边覆盖区域减去右边覆盖区域，并求出所有像素点灰度差的绝对值之和；
5. 移动右边图像的窗口，重复（3）-（4）的处理（这里有个搜索范围,超过这个范围跳出）；
6. 找到这个范围内SAD值最小的窗口,即找到了左图锚点的最佳匹配的像素块。
   

参考链接：https://blog.csdn.net/u012507022/article/details/51446891