orb-slam系列 Tracking线程 匹配(四)

ComputeStereoMatches() 函数

1）在同一行，设定一个阈值，找到大致的位置
2）在范围内所有的目标关键点进行甄别，需要满足条件在合理的视差范围内，并且金字塔层数不能超过一

    const float minZ = mb;   //人为设置最小深度
    const float minD = 0;    //最小视差
    const float maxD = mbf/minZ;    //最大视差

3）计算符合的描述子距离，选择最佳描述子。
4）SAD匹配提高像素匹配修正量bestincR

// 通过SAD匹配提高像素匹配修正量bestincR
        if(bestDist<thOrbDist)
        {
            // coordinates in image pyramid at keypoint scale
            // kpL.pt.x对应金字塔最底层坐标，将最佳匹配的特征点对尺度变换到尺度对应层 (scaleduL, scaledvL) (scaleduR0, )
            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);
 
            // sliding window search
            const int w = 5; 
            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;
            const int L = 5;
            vector<float> vDists;
            vDists.resize(2*L+1); // 11
 
            // 滑动窗口的滑动范围为（-L, L）,提前判断滑动窗口滑动过程中是否会越界
            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++)
            {
                // 横向滑动窗口
                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); 
                float dist = cv::norm(IL,IR,cv::NORM_L1); // 一范数，计算差的绝对值
                if(dist<bestDist)
                {
                    bestDist =  dist;// SAD匹配目前最小匹配偏差
                    bestincR = incR; // SAD匹配目前最佳的修正量
                }
 
                vDists[L+incR] = dist; // 正常情况下，这里面的数据应该以抛物线形式变化
            }
 
            if(bestincR==-L || bestincR==L) // SAD匹配失败，同时放弃求该特征点的深度
                continue;
 
            // Sub-pixel match (Parabola fitting)
            // 做抛物线拟合找谷底得到亚像素匹配deltaR
            // (bestincR,dist) (bestincR-1,dist) (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匹配最小匹配偏差
            }

特征点选取完毕

ComputeImageBounds()

/*****************************************************************
 *     函数属性：类Frame的成员函数ComputeImageBounds(const cv::Mat &imLeft)
 *     函数功能：
 *                 函数分为两部分，一部分是当图片需要矫正时：图像的边界为矫正后的图像边界
 *                 第二部分是函数不需要矫正时 图像的边界就是原图像的边界
 *                 此函数的最终结果为：将图形的边界赋值即mnMinX、mnMaxX、mnMinY、mnMaxY
 *     函数参数介绍：
 *                         imLeft：图像彩色图对应的灰度图
 *     备注：计算图像边界（在初始化时调用）
 ******************************************************************/
void Frame::ComputeImageBounds(const cv::Mat &imLeft)
{
    if(mDistCoef.at<float>(0)!=0.0)  //如果图片需要失真矫正
    {
        // 矫正前四个边界点：(0,0) (cols,0) (0,rows) (cols,rows)
        cv::Mat mat(4,2,CV_32F);
        mat.at<float>(0,0)=0.0; mat.at<float>(0,1)=0.0;
        mat.at<float>(1,0)=imLeft.cols; mat.at<float>(1,1)=0.0;
        mat.at<float>(2,0)=0.0; mat.at<float>(2,1)=imLeft.rows;
        mat.at<float>(3,0)=imLeft.cols; mat.at<float>(3,1)=imLeft.rows;

        // Undistort corners  对rgb图进行失真矫正
        mat=mat.reshape(2);
        cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);   //将矫正后的图像放入mat中
        mat=mat.reshape(1);

        mnMinX = min(mat.at<float>(0,0),mat.at<float>(2,0));  //左上和左下横坐标最小的
        mnMaxX = max(mat.at<float>(1,0),mat.at<float>(3,0)); //右上和右下横坐标最大的
        mnMinY = min(mat.at<float>(0,1),mat.at<float>(1,1));  //左上和右上纵坐标最小的
        mnMaxY = max(mat.at<float>(2,1),mat.at<float>(3,1)); //左下和右下纵坐标最小的

    }
    else          //如果图片不需要失真矫正
    {
        mnMinX = 0.0f;
        mnMaxX = imLeft.cols;
        mnMinY = 0.0f;
        mnMaxY = imLeft.rows;
    }
}

AssignFeaturesToGrid()

/*****************************************************************
*      函数属性：类Frame的成员函数AssignFeaturesToGrid()
 *     函数功能：
 *                 将整张图片分为64×48的网格
 *                 并将每个特征点的id加入到该网格中，即mGrid容器存储的是特征点的id
 *     函数参数介绍：NULL
 *     备注：分配特征点到各个网格，加速特征匹配
 ******************************************************************/
void Frame::AssignFeaturesToGrid()
{
    int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
    for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
        for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
            mGrid[i][j].reserve(nReserve);   //给每个网格预留下空间，为什么要预留这些？
    //将特征点分配到这些网格中
    for(int i=0;i<N;i++)
    {
        const cv::KeyPoint &kp = mvKeysUn[i];

        int nGridPosX, nGridPosY;  //存储网格位置，证明第(nGridPosX,nGridPosY)个网格
        if(PosInGrid(kp,nGridPosX,nGridPosY))    //如果第i个特征点位置在第(nGridPosX,nGridPosY)个网格中，就将该特征点的id存入该网格中
            mGrid[nGridPosX][nGridPosY].push_back(i);
    }
}