【SLAM】ORB-SLAM3解析——帧Frame()的构建(2)

在路径\src\Frame.cc下。以Stereo(左右相机模型不同)-IMU模式的构造函数(1034行)为例进入到它的内部。

这部分的主体思路：

对于每一帧image(可能是mono，stereo， RGBD)，首先构建图像金字塔，从而方便提取不同尺度的特征点和找到不同尺度特征点间的匹配关系。然后在image上提取均匀分布的FAST角点，为了实现这一点，对照片分成若干个格子，在不同的格子下分别提取角点；另外，运用BFS的方式不断把有角点的区域四等份，直到每个区域只有一个角点。随后对于每一个角点的周围区域，计算它的灰度质心，连接几何中心到灰度质心，就能找到该特征点的方向；在这个方向上，再找一个区域，根据提前设定的模板找到256个像素对，比较每一个像素对的灰度关系从而得到当前特征点的ORB描述子。对于stereo模型，需要找到双目间的匹配关系并恢复深度，如果是相同相机模型，运用级线约束+描述子匹配+块匹配+亚像素插值来实现；对于双目是鱼眼或者不同相机模型，那么通过描述子距离+Lowe's ratio实现这个目的。总的来说还是很复杂的。

总而言之，就干了3件事，提FAST角点，画ORB描述子，恢复camera系下深度值。

首先第一个问题，Frame这几个构造函数怎么选？根据传入参数的类型和数量，可以确定。我们进入到Frame.h中，可以看到Frame一共有4个构造函数：

62行：stereo，且左右相机模型相同
65行：RGBD
68行：mono
346行：stereo，且左右相机模型不同(本篇以这个为例)

不同的构造函数内部，结构是相似的，但是部分函数是否出现，调用顺序是有差异的。

2.1 Frame的构造

2.1.1 ORBextractor的构造

在创建Frame对象的时候，从外部传入了ORBextractor对象，而这个在Frame里面有着非常重要的作用，它是在Tracking::Tracking()中的newParameterLoader(settings)中创建的，我们进入到它的构造函数：传入参数包括提取的特征点总个数，缩放系数，图层数，和FAST提取的两个不同的阈值。

ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,
                           int _iniThFAST, int _minThFAST):
        nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
        iniThFAST(_iniThFAST), minThFAST(_minThFAST)

根据图层数和相邻层之间的缩放系数，获取每一个图层相对于第一层的缩放系数：

    mvScaleFactor.resize(nlevels);
    mvLevelSigma2.resize(nlevels);
    mvScaleFactor[0]=1.0f;
    mvLevelSigma2[0]=1.0f;
    for(int i=1; i

初始化图像金字塔：

    mvImagePyramid.resize(nlevels);

确定每一个图层的面积占总面积的比例确定当前图层需要提取多少个FAST角点：

    mnFeaturesPerLevel.resize(nlevels);
    float factor = 1.0f / scaleFactor;
    float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));

    int sumFeatures = 0;
    for( int level = 0; level < nlevels-1; level++ )
    {
        mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);
        sumFeatures += mnFeaturesPerLevel[level];
        nDesiredFeaturesPerScale *= factor;
    }
    mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);

接下来这部分内容跟计算ORB描述子相关，见2.2.2.3：

    //This is for orientation
    // pre-compute the end of a row in a circular patch
    umax.resize(HALF_PATCH_SIZE + 1);

    int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);
    int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);
    const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;
    for (v = 0; v <= vmax; ++v) umax[v] = cvRound(sqrt(hp2 - v * v));

    // Make sure we are symmetric
    for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
    {
        while (umax[v0] == umax[v0 + 1]) ++v0;
        umax[v] = v0;
        ++v0;
    }
}

2.1.2 Frame的构造

首先获取照片，更新帧id，获取图像金字塔每层的缩放系数：

    imgLeft = imLeft.clone();
    imgRight = imRight.clone();

    // Frame ID
    mnId=nNextId++;

    // Scale Level Info
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor = log(mfScaleFactor);
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

然后提取ORB特征：

    thread threadLeft(&Frame::ExtractORB,this,0,imLeft,static_cast(mpCamera)->mvLappingArea[0],static_cast(mpCamera)->mvLappingArea[1]);
    thread threadRight(&Frame::ExtractORB,this,1,imRight,static_cast(mpCamera2)->mvLappingArea[0],static_cast(mpCamera2)->mvLappingArea[1]);

如果是第一帧，计算去畸变的边界：

    if(mbInitialComputations)
    {
        ComputeImageBounds(imLeft);

        mfGridElementWidthInv=static_cast(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
        mfGridElementHeightInv=static_cast(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);

        fx = K.at(0,0);
        fy = K.at(1,1);
        cx = K.at(0,2);
        cy = K.at(1,2);
        invfx = 1.0f/fx;
        invfy = 1.0f/fy;

        mbInitialComputations=false;
    }

获取左右特征的匹配关系：

    ComputeStereoFishEyeMatches();   

初始化本帧的地图点：

    mvpMapPoints = vector(N,static_cast(NULL));
    mvbOutlier = vector(N,false);

把特征点放到对应的格子里：

    AssignFeaturesToGrid();

获得去畸变的坐标：

    UndistortKeyPoints();

  
2.2 ORB特征的获取

参考链接：
https://zhuanlan.zhihu.com/p/61738607
https://blog.csdn.net/weixin_45947476/article/details/122799429

这一部分的作用就是在当前照片上，提取尽可能平均分布的FAST角点，同时计算他们ORB描述子。所涉及的所有内容都在ORBextractor类下。

void Frame::ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1)
{
    vector vLapping = {x0,x1};
    if(flag==0)
        monoLeft = (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors,vLapping);
    else
        monoRight = (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight,vLapping);
}

进入到ORBextractor::operator()，首先他会计算图片金字塔：

        // Pre-compute the scale pyramid
        ComputePyramid(image);

按照四叉树的方式提取ORB特征：

        vector < vector > allKeypoints;
        ComputeKeyPointsOctTree(allKeypoints);

初始化描述子容器，他是一个nkeypoints行，32*8=256列的Matrix：

        Mat descriptors;
        int nkeypoints = 0;
        for (int level = 0; level < nlevels; ++level)
            nkeypoints += (int)allKeypoints[level].size();
        if( nkeypoints == 0 )
            _descriptors.release();
        else
        {
            _descriptors.create(nkeypoints, 32, CV_8U);
            descriptors = _descriptors.getMat();
        }

然后遍历每一个图层，对当前图层的照片进行GaussianBlur，再计算描述子：

        int offset = 0;
        //Modified for speeding up stereo fisheye matching
        int monoIndex = 0, stereoIndex = nkeypoints-1;
        for (int level = 0; level < nlevels; ++level)
        {
            vector& keypoints = allKeypoints[level];
            int nkeypointsLevel = (int)keypoints.size();

            if(nkeypointsLevel==0) continue;

            // preprocess the resized image
            Mat workingMat = mvImagePyramid[level].clone();
            GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);

            // Compute the descriptors
            Mat desc = cv::Mat(nkeypointsLevel, 32, CV_8U);
            computeDescriptors(workingMat, keypoints, desc, pattern);

            offset += nkeypointsLevel;

再进一步遍历当前图层上的每一个特征点，根据对应的缩放系数缩放它的像素坐标，再根据它与LappingArea的关系，确定当前特征点描述子在总描述子容器中的位置，如果是keypoint->pt.x >= vLappingArea[0] && keypoint->pt.x <= vLappingArea[1]，那么它的描述子放在后面，否则放在前面。它的目的是为了应对左右相机模型不同这个情况，lapping area是左右照片的共同可视范围。如果是单目或者左右目是相同相机的情况，那么vLappingArea[0]==vLappingArea[1]==0。

            float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor);
            int i = 0;
            for (vector::iterator keypoint = keypoints.begin(),
                         keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint){

                // Scale keypoint coordinates
                if (level != 0){
                    keypoint->pt *= scale;
                }

                if(keypoint->pt.x >= vLappingArea[0] && keypoint->pt.x <= vLappingArea[1]){
                    _keypoints.at(stereoIndex) = (*keypoint);
                    desc.row(i).copyTo(descriptors.row(stereoIndex));
                    stereoIndex--;
                }
                else{
                    _keypoints.at(monoIndex) = (*keypoint);
                    desc.row(i).copyTo(descriptors.row(monoIndex));
                    monoIndex++;
                }
                i++;
            }
        }
        return monoIndex;
    }

    
2.2.1 构造图片金字塔ComputePyramid(image)

因为我自己目前没有做相关的实验，这部分内容主要参考别人的博客进行理解。在这里，作者的原意是要对每一层照片外圈进行padding操作，但是代码里temp的确是扩充边界了，但是没有传递给mvImagePyramid，所以图像金字塔只是单纯进行了缩放赋值，没有进行padding。

void ORBextractor::ComputePyramid(cv::Mat image)
{
    //开始遍历所有的图层
    for (int level = 0; level < nlevels; ++level)
    {
        //获取本层图像的缩放系数
        float scale = mvInvScaleFactor[level];
        //计算本层图像的像素尺寸大小
        Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
        //全尺寸图像。包括无效图像区域的大小。将图像进行“补边”，EDGE_THRESHOLD区域外的图像不进行FAST角点检测
        Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);
        // 定义了两个变量：temp是扩展了边界的图像，masktemp并未使用
        Mat temp(wholeSize, image.type()), masktemp;
        // mvImagePyramid 刚开始时是个空的vector
        // 把图像金字塔该图层的图像指针mvImagePyramid指向temp的中间部分（这里为浅拷贝，内存相同）
        mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));

        // Compute the resized image
        //计算第0层以上resize后的图像
        if( level != 0 )
        {
            //将上一层金字塔图像根据设定sz缩放到当前层级
            resize(mvImagePyramid[level-1],    //输入图像
                   mvImagePyramid[level],     //输出图像
                   sz,                         //输出图像的尺寸
                   0,                         //水平方向上的缩放系数，留0表示自动计算
                   0,                          //垂直方向上的缩放系数，留0表示自动计算
                   cv::INTER_LINEAR);        //图像缩放的差值算法类型，这里的是线性插值算法

            //把源图像拷贝到目的图像的中央，四面填充指定的像素。图片如果已经拷贝到中间，只填充边界
            //这样做是为了能够正确提取边界的FAST角点
            //EDGE_THRESHOLD指的这个边界的宽度，由于这个边界之外的像素不是原图像素而是算法生成出来的，所以不能够在EDGE_THRESHOLD之外提取特征点            
            copyMakeBorder(mvImagePyramid[level],                     //源图像
                           temp,                                     //目标图像（此时其实就已经有大了一圈的尺寸了）
                           EDGE_THRESHOLD, EDGE_THRESHOLD,             //top & bottom 需要扩展的border大小
                           EDGE_THRESHOLD, EDGE_THRESHOLD,            //left & right 需要扩展的border大小
                           BORDER_REFLECT_101+BORDER_ISOLATED);     //扩充方式，opencv给出的解释：
            
            /*Various border types, image boundaries are denoted with '|'
            * BORDER_REPLICATE:     aaaaaa|abcdefgh|hhhhhhh  重复
            * BORDER_REFLECT:       fedcba|abcdefgh|hgfedcb  反射
            * BORDER_REFLECT_101:   gfedcb|abcdefgh|gfedcba  反射101，相当于上一行的左右互换
            * BORDER_WRAP:          cdefgh|abcdefgh|abcdefg  外包装
            * BORDER_CONSTANT:      iiiiii|abcdefgh|iiiiiii  with some specified 'i'  常量
            */
            
            //BORDER_ISOLATED    表示对整个图像进行操作
            // https://docs.opencv.org/3.4.4/d2/de8/group__core__array.html#ga2ac1049c2c3dd25c2b41bffe17658a36
        }
        else
        {
            //对于第0层未缩放图像，直接将图像深拷贝到temp的中间，并且对其周围进行边界扩展。此时temp就是对原图扩展后的图像
            copyMakeBorder(image,            //这里是原图像
                           temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
                           BORDER_REFLECT_101);            
        }
    }
}

 在这里出现了两个特殊的值，EDGE_THRESHOLD和3，参考该博客中第四部分的图片，对于当前图层的照片，往外扩大了EDGE_THRESHOLD层，这部分的作用是为了后面的高斯模糊；而提取FAST角点，需要往外扩3层。所以当前只有中间部分是原始有效的照片。不过话虽这么说，但是在ComputePyramid()里，并没有看到把padding的操作传递给图像金字塔...所以这里只是单纯的放弃了照片的边界，或者是我比较菜有地方漏掉了？

2.2.2 提取ORB特征ComputeKeyPointsOctTree(allKeypoints)

2.2.2.1 提取FAST角点

这部分的核心思想就是对于图像金字塔的每一层，都平均分成大小相同的grid，在每一个grid上提取FAST角点。这么做的目的是让提取的角点分布比较均匀。

void ORBextractor::ComputeKeyPointsOctTree(vector >& allKeypoints)    
{
    allKeypoints.resize(nlevels); //给存放特征点的容器分配空间
    const float W = 35;           //一个grid的尺寸，这是提取FAST角点的最小图片单位

    //遍历所有图像
    for (int level = 0; level < nlevels; ++level)
    {
        //计算这层图像的坐标边界
        const int minBorderX = EDGE_THRESHOLD-3;            //这里的3是因为在计算FAST特征点的时候，需要建立一个半径为3的圆
        const int minBorderY = minBorderX;                    //minY的计算就可以直接拷贝上面的计算结果了
        const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
        const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;

        vector vToDistributeKeys;  //存放当前图层提取的特征点的容器
        vToDistributeKeys.reserve(nfeatures*10); //这里预分配的空间大小是nfeatures*10

        //这层图像的长和宽
        const float width = (maxBorderX-minBorderX);
        const float height = (maxBorderY-minBorderY);

        //在当前层的图像有多少行和多少列grid
        const int nCols = width/W;
        const int nRows = height/W;
        //计算每个grid所占的像素行数和列数
        const int wCell = ceil(width/nCols);
        const int hCell = ceil(height/nRows);

现在grid分配好了，那么就可以遍历每一个grid，在上面提取FAST角点。当然了，对于处于边界的grid，尺寸会小一些：

        for(int i=0; i=maxBorderY-3)  continue;
            if(maxY>maxBorderY) maxY = maxBorderY;

            for(int j=0; j=maxBorderX-6) continue;
                if(maxX>maxBorderX) maxX = maxBorderX;

首先先根据iniThFAST提一波FAST角点，如果没提出来，说明阈值太严了，放松阈值再提一波：

                vector vKeysCell;  //存储这个格子中的特征点
                FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),    //待检测的图像，这里就是当前遍历到的图像块
                     vKeysCell,            //存储角点位置的容器
                     iniThFAST,            //检测阈值
                     true);                //使能非极大值抑制

                //如果这个图像块中使用默认的FAST检测阈值没有能够检测到角点
                if(vKeysCell.empty())
                {
                    //那么就使用更低的阈值来进行重新检测
                    FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),    //待检测的图像
                         vKeysCell,        //存储角点位置的容器
                         minThFAST,        //更低的检测阈值
                         true);            //使能非极大值抑制
                }

因为提取的像素坐标都是在当前格子里的局部坐标，需要转换到当前图层下的全局坐标：

                if(!vKeysCell.empty())
                {
                    for(vector::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
                    {
                        //NOTICE 到目前为止，这些角点的坐标都是基于图像cell的，现在我们要先将其恢复到当前的【坐标边界】下的坐标
                        (*vit).pt.x+=j*wCell;
                        (*vit).pt.y+=i*hCell;
                        vToDistributeKeys.push_back(*vit);
                    }
                }
            }
        }

2.2.2.2 每一个区域只保留最大响应的FAST角点使角点均匀分布

这一部分的核心思想是对上一步提取的角点进行过滤，让角点分布的更加均匀。怎么办？四叉树！把图片分成4个部分，如果某一部分没有点，那么删除这个部分；如果某一部分点数>1，那么继续分下去；如果某一部分只有一个点，那么也不继续分下去了。

思路具体为：
    1、如果图片的宽度比较宽，就先把分成左右w/h份。一般的640×480的图像开始的时候只有一个node。
    2、如果node里面的点数>1，把每个node分成四个node，如果node里面的特征点为空，就不要了，删掉。
    3、新分的node的点数>1，就再分裂成4个node。如此，一直分裂。
    4、终止条件为：node的总数量> [公式] ，或者无法再进行分裂。
    5、然后从每个node里面选择一个质量最好的FAST点。

Step 1：根据宽高比确定初始节点数目

vector ORBextractor::DistributeOctTree(const vector& vToDistributeKeys, const int &minX,
                                       const int &maxX, const int &minY, const int &maxY, const int &N, const int &level)
{
    // Compute how many initial nodes
    //计算应该生成的初始节点个数，根节点的数量nIni是根据边界的宽高比值确定的，一般是1或者2
    // ! bug: 如果宽高比小于0.5，nIni=0, 后面hx会报错
    const int nIni = round(static_cast(maxX-minX)/(maxY-minY)); //此处的maxX等在“ORB提取扩展图像”的图片上，round为取整。

    //一个初始的节点的x方向有多少个像素
    const float hX = static_cast(maxX-minX)/nIni;

    //存储有提取器节点的链表
    list lNodes;

    //存储初始提取器节点指针的vector
    vector vpIniNodes;

    //重新设置其大小
    vpIniNodes.resize(nIni);

Step 2：生成初始提取器节点

    for(int i=0; i(i),0);    //UpLeft
        ni.UR = cv::Point2i(hX*static_cast(i+1),0);  //UpRight
        ni.BL = cv::Point2i(ni.UL.x,maxY-minY);                //BottomLeft
        ni.BR = cv::Point2i(ni.UR.x,maxY-minY);             //BottomRight

        //重设vkeys大小
        ni.vKeys.reserve(vToDistributeKeys.size());

        //将刚才生成的提取节点添加到链表中
        lNodes.push_back(ni);
        //存储这个初始的提取器节点句柄
        vpIniNodes[i] = &lNodes.back();
    }

Step 3：将特征点分配到子提取器节点中

    //Associate points to childs
    for(size_t i=0;ivKeys.push_back(kp);
    }

 Step 4：遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点

    list::iterator lit = lNodes.begin();
    while(lit!=lNodes.end())
    {
        //如果初始的提取器节点所分配到的特征点个数为1
        if(lit->vKeys.size()==1)
        {
            //那么就标志位置位，表示此节点不可再分
            lit->bNoMore=true;
            //更新迭代器
            lit++;
        }
        ///如果一个提取器节点没有被分配到特征点，那么就从列表中直接删除它
        else if(lit->vKeys.empty())
            //注意，由于是直接删除了它，所以这里的迭代器没有必要更新；否则反而会造成跳过元素的情况
            lit = lNodes.erase(lit);            
        else
            //如果上面的这些情况和当前的特征点提取器节点无关，那么就只是更新迭代器 
            lit++;
    }

以上4步都是对初始化分配的节点的操作，接下来就是广度优先循环进行节点的划分，直到满足迭代停止条件。

Step 5：利用四叉树方法对图像进行划分区域，均匀分配特征点

    bool bFinish = false;  //结束标志位清空
    int iteration = 0;     //记录迭代次数，只是记录，并未起到作用
    vector > vSizeAndPointerToNode;  //这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和节点指针
    vSizeAndPointerToNode.reserve(lNodes.size()*4);           //调整大小，这是当所有节点都能分成4份时的大小
    while(!bFinish)
    {
        iteration++;
        int prevSize = lNodes.size();//保存当前节点个数
        lit = lNodes.begin();        //重新定位迭代器指向列表头部
        int nToExpand = 0;           //需要展开的节点计数，这个一直保持累计，不清零
        vSizeAndPointerToNode.clear();

        //开始遍历列表中所有的提取器节点，并进行分解或者保留
        while(lit!=lNodes.end())
        {
            //如果提取器节点只有一个特征点，
            if(lit->bNoMore)
            {
                // If node only contains one point do not subdivide and continue
                //那么就没有必要再进行细分了
                lit++;
                //跳过当前节点，继续下一个
                continue;
            }
            else
            {
                // If more than one point, subdivide
                //如果当前的提取器节点具有超过一个的特征点，那么就要进行继续分裂
                ExtractorNode n1,n2,n3,n4;

                //再细分成四个子区域，DivideNode函数是进行结点划分
                lit->DivideNode(n1,n2,n3,n4); 

                // Add childs if they contain points
                //如果这里分出来的子区域中有特征点，那么就将这个子区域的节点添加到提取器节点的列表中
                //注意这里的条件是，有特征点即可
                if(n1.vKeys.size()>0)
                {
                    //注意这里也是添加到列表前面的
                    lNodes.push_front(n1);   

                    //再判断其中子提取器节点中的特征点数目是否大于1
                    if(n1.vKeys.size()>1)
                    {
                        //如果有超过一个的特征点，那么待展开的节点计数加1
                        nToExpand++;

                        //保存这个特征点数目和节点指针的信息
                        vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));

                        //?这个访问用的句柄貌似并没有用到？
                        // lNodes.front().lit 和前面的迭代的lit 不同，只是名字相同而已
                        // lNodes.front().lit是node结构体里的一个指针用来记录节点的位置
                        // 迭代的lit 是while循环里作者命名的遍历的指针名称
                        lNodes.front().lit = lNodes.begin();
                    }
                }
                //后面的操作都是相同的，这里不再赘述
                if(n2.vKeys.size()>0)
                {
                    lNodes.push_front(n2);
                    if(n2.vKeys.size()>1)
                    {
                        nToExpand++;
                        vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
                        lNodes.front().lit = lNodes.begin();
                    }
                }
                if(n3.vKeys.size()>0)
                {
                    lNodes.push_front(n3);
                    if(n3.vKeys.size()>1)
                    {
                        nToExpand++;
                        vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
                        lNodes.front().lit = lNodes.begin();
                    }
                }
                if(n4.vKeys.size()>0)
                {
                    lNodes.push_front(n4);
                    if(n4.vKeys.size()>1)
                    {
                        nToExpand++;
                        vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
                        lNodes.front().lit = lNodes.begin();
                    }
                }

                //当这个母节点expand之后就从列表中删除它了，能够进行分裂操作说明至少有一个子节点的区域中特征点的数量是>1的
                // 分裂方式是后加的节点先分裂，先加的后分裂
                lit=lNodes.erase(lit);

                //继续下一次循环，其实这里加不加这句话的作用都是一样的
                continue;
            }
        }

        // Finish if there are more nodes than required features or all nodes contain just one point
        //停止这个过程的条件有两个，满足其中一个即可：
        //1、当前的节点数已经超过了要求的特征点数
        //2、当前所有的节点中都只包含一个特征点
        if((int)lNodes.size()>=N                 //判断是否超过了要求的特征点数
            || (int)lNodes.size()==prevSize)    //prevSize中保存的是分裂之前的节点个数，如果分裂之前和分裂之后的总节点个数一样，说明当前所有的
                                                //节点区域中只有一个特征点，已经不能够再细分了
        {
            //停止标志置位
            bFinish = true;
        }

Step 6：当再划分之后所有的Node数大于要求数目时,就慢慢划分直到使其刚刚达到或者超过要求的特征点个数

        //可以展开的子节点个数nToExpand x3，是因为一分四之后，会删除原来的主节点，所以乘以3
        /**
         * 注意到，这里的nToExpand变量在前面的执行过程中是一直处于累计状态的，如果因为特征点个数太少，跳过了下面的else-if，又进行了一次上面的遍历
         * list的操作之后，lNodes.size()增加了，但是nToExpand也增加了，尤其是在很多次操作之后，下面的表达式：
         * ((int)lNodes.size()+nToExpand*3)>N
         * 会很快就被满足，但是此时只进行一次对vSizeAndPointerToNode中点进行分裂的操作是肯定不够的；
         * 理想中，作者下面的for理论上只要执行一次就能满足，不过作者所考虑的“不理想情况”应该是分裂后出现的节点所在区域可能没有特征点，因此将for
         * 循环放在了一个while循环里面，通过再次进行for循环、再分裂一次解决这个问题。而我所考虑的“不理想情况”则是因为前面的一次对vSizeAndPointerToNode
         * 中的特征点进行for循环不够，需要将其放在另外一个循环（也就是作者所写的while循环）中不断尝试直到达到退出条件。 
         * */
        else if(((int)lNodes.size()+nToExpand*3)>N)
        {
            //如果再分裂一次那么数目就要超了，这里想办法尽可能使其刚刚达到或者超过要求的特征点个数时就退出
            //这里的nToExpand和vSizeAndPointerToNode不是一次循环对一次循环的关系，而是前者是累计计数，后者只保存某一个循环的
            //一直循环，直到结束标志位被置位
            while(!bFinish)
            {
                //获取当前的list中的节点个数
                prevSize = lNodes.size();
                //保留那些还可以分裂的节点的信息, 这里是深拷贝
                vector > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
                //清空
                vSizeAndPointerToNode.clear();

                // 对需要划分的节点进行排序，对pair对的第一个元素进行排序，默认是从小到大排序
                // 优先分裂特征点多的节点，使得特征点密集的区域保留更少的特征点
                //! 注意这里的排序规则非常重要！会导致每次最后产生的特征点都不一样。建议使用 stable_sort
                sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());

                //遍历这个存储了pair对的vector，注意是从后往前遍历
                for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--)
                {
                    ExtractorNode n1,n2,n3,n4;
                    //对每个需要进行分裂的节点进行分裂
                    vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4);

                    // Add childs if they contain points
                    //其实这里的节点可以说是二级子节点了，执行和前面一样的操作
                    if(n1.vKeys.size()>0)
                    {
                        lNodes.push_front(n1);
                        if(n1.vKeys.size()>1)
                        {
                            //因为这里还有对于vSizeAndPointerToNode的操作，所以前面才会备份vSizeAndPointerToNode中的数据
                            //为可能的、后续的又一次for循环做准备
                            vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }
                    if(n2.vKeys.size()>0)
                    {
                        lNodes.push_front(n2);
                        if(n2.vKeys.size()>1)
                        {
                            vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }
                    if(n3.vKeys.size()>0)
                    {
                        lNodes.push_front(n3);
                        if(n3.vKeys.size()>1)
                        {
                            vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }
                    if(n4.vKeys.size()>0)
                    {
                        lNodes.push_front(n4);
                        if(n4.vKeys.size()>1)
                        {
                            vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
                            lNodes.front().lit = lNodes.begin();
                        }
                    }

                    //删除母节点，在这里其实应该是一级子节点
                    lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit);

                    //判断是是否超过了需要的特征点数？是的话就退出，不是的话就继续这个分裂过程，直到刚刚达到或者超过要求的特征点个数
                    //作者的思想其实就是这样的，再分裂了一次之后判断下一次分裂是否会超过N，如果不是那么就放心大胆地全部进行分裂（因为少了一个判断因此
                    //其运算速度会稍微快一些），如果会那么就引导到这里进行最后一次分裂
                    if((int)lNodes.size()>=N) break;
                }//遍历vPrevSizeAndPointerToNode并对其中指定的node进行分裂，直到刚刚达到或者超过要求的特征点个数

                //这里理想中应该是一个for循环就能够达成结束条件了，但是作者想的可能是，有些子节点所在的区域会没有特征点，因此很有可能一次for循环之后
                //的数目还是不能够满足要求，所以还是需要判断结束条件并且再来一次
                //判断是否达到了停止条件
                if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
                    bFinish = true;                
            }//一直进行nToExpand累加的节点分裂过程，直到分裂后的nodes数目刚刚达到或者超过要求的特征点数目
        }//当本次分裂后达不到结束条件但是再进行一次完整的分裂之后就可以达到结束条件时
    }// 根据兴趣点分布,利用4叉树方法对图像进行划分区域

Step 7：保留每个区域响应值最大的一个兴趣点

    //使用这个vector来存储我们感兴趣的特征点的过滤结果
    vector vResultKeys;

    //调整容器大小为要提取的特征点数目
    vResultKeys.reserve(nfeatures);

    //遍历这个节点链表
    for(list::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
    {
        //得到这个节点区域中的特征点容器句柄
        vector &vNodeKeys = lit->vKeys;

        //得到指向第一个特征点的指针，后面作为最大响应值对应的关键点
        cv::KeyPoint* pKP = &vNodeKeys[0];

        //用第1个关键点响应值初始化最大响应值
        float maxResponse = pKP->response;

        //开始遍历这个节点区域中的特征点容器中的特征点，注意是从1开始哟，0已经用过了
        for(size_t k=1;kmaxResponse)
            {
                //更新pKP指向具有最大响应值的keypoints
                pKP = &vNodeKeys[k];
                maxResponse = vNodeKeys[k].response;
            }
        }
        //将这个节点区域中的响应值最大的特征点加入最终结果容器
        vResultKeys.push_back(*pKP);
    }
    //返回最终结果容器，其中保存有分裂出来的区域中，我们最感兴趣、响应值最大的特征点
    return vResultKeys;
}

2.2.2.3 计算FAST角点的方向

首先先看一下参考链接里画的那个图，我们想遍历以某个点为圆心的一个圆内所有点的坐标，
那么对于给定的第v行，我们需要计算当前行哪几列是在圆的范围内，那么找到列的最大值umax
就可以确定列的范围。考虑到圆是对称的，所以我们只需要遍历1/4个圆就可以确定整个圆的范围。对于这个1/4个圆，再按照45°那个方向把他划分成1/8个圆，如图ADE表示的这个1/4个圆，我们在E，D点分别开始向B点移动相同的步长，此时两点的u，v值恰好是对方的v，u值。这么说有点绕，我们回到ORBextractor::ORBextractor()构造函数的最后面，看着这张图，看看他是怎么找到圆的范围的。

    //因为我们只看1/4个圆ADE，所以umax的尺寸为 半径+1
    //umax的意思是，如果当前是第v行，那么当前行所能有的最大列是多少
    umax.resize(HALF_PATCH_SIZE + 1);
    
    //cvFloor返回不大于参数的最大整数值，cvCeil返回不小于参数的最小整数值，cvRound则是四舍五入
    int v,        //行
        v0,        //初始位置的行
        vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);    //计算圆的最大行号，+1应该是把中间行也给考虑进去了
                //NOTICE 注意这里的最大行号指的是计算的时候的最大行号，此行的和圆的角点在45°圆心角的一边上，之所以这样选择
                //是因为圆周上的对称特性，也就是上图中B点对应的行号            

    //这里的二分之根2就是对应那个45°圆心角，和vmax是同一个位置B点 
    int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);
    //半径的平方
    const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;

    //从D点向E点运动，对于第v行，它的列的最大值为sqrt(hp2 - v * v)
    for (v = 0; v <= vmax; ++v)
        umax[v] = cvRound(sqrt(hp2 - v * v));        //结果都是大于0的结果，表示x坐标在这一行的边界

    // Make sure we are symmetric
    //这里其实是使用了对称的方式计算上四分之一的圆周上的umax，目的也是为了保持严格的对称（如果按照常规的想法做，由于cvRound就会很容易出现不对称的情况，
    //同时这些随机采样的特征点集也不能够满足旋转之后的采样不变性了）
    //初始位置时，v对应上图E点，v0对应上图D点，此时同时向B点运动，因此运动到B点为迭代停止条件
    for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
    {
        //因为误差的存在，在同一圆内D点附近可能若干行对应的列数是相同的，此时v0继续向B方向运动
        while (umax[v0] == umax[v0 + 1])
            ++v0;
        //因为1/4圆ADE关于AB对称，v0在D附近增加的行数恰好等于v在E附近增加的列数
        umax[v] = v0;
        DEBUG("%d=%d", v, v0);
        ++v0;
    }

我们再回来看看如何计算每一个图层特征点的方向的：

/**
 * @brief 计算特征点的方向
 * @param[in] image                 特征点所在当前金字塔的图像
 * @param[in & out] keypoints       特征点向量
 * @param[in] umax                  每个特征点所在图像区块的每行的边界 u_max 组成的vector
 */
static void computeOrientation(const Mat& image, vector& keypoints, const vector& umax)
{
    // 遍历所有的特征点
    for (vector::iterator keypoint = keypoints.begin(),
         keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
    {
        // 调用IC_Angle 函数计算这个特征点的方向
        keypoint->angle = IC_Angle(image,             //特征点所在的图层的图像
                                   keypoint->pt,     //特征点在这张图像中的坐标
                                   umax);            //每个特征点所在图像区块的每行的边界 u_max 组成的vector
    }
}

/**
 * @brief 这个函数用于计算特征点的方向，这里是返回角度作为方向。
 * 计算特征点方向是为了使得提取的特征点具有旋转不变性。
 * 方法是灰度质心法：以几何中心和灰度质心的连线作为该特征点方向
 * @param[in] image     要进行操作的某层金字塔图像
 * @param[in] pt        当前特征点的坐标
 * @param[in] u_max     图像块的每一行的坐标边界 u_max
 * @return float        返回特征点的角度，范围为[0,360)角度，精度为0.3°
 */
static float IC_Angle(const Mat& image, Point2f pt,  const vector & u_max)
{
    //图像的矩，前者是按照图像块的y坐标加权，后者是按照图像块的x坐标加权
    int m_01 = 0, m_10 = 0;

    //获得这个特征点所在的图像块的中心点坐标灰度值的指针center
    const uchar* center = &image.at (cvRound(pt.y), cvRound(pt.x));

    // Treat the center line differently, v=0
    //这条v=0中心线的计算需要特殊对待
    //后面是以中心行为对称轴，成对遍历行数，所以PATCH_SIZE必须是奇数
    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
        //注意这里的center下标u可以是负的！中心水平线上的像素按x坐标（也就是u坐标）加权
        m_10 += u * center[u];

    // Go line by line in the circular patch  
    //这里的step1表示这个图像一行包含的字节总数。参考[https://blog.csdn.net/qianqing13579/article/details/45318279]
    int step = (int)image.step1();
    //注意这里是以行v=0中心线为对称轴，然后对称地每成对的两行之间进行遍历，这样处理加快了计算速度
    for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
    {
        // Proceed over the two lines
        //本来m_01应该是一行一行地计算的，但是由于对称以及坐标x,y正负的原因，可以一次计算两行
        int v_sum = 0;
        // 获取某列像素横坐标的最大范围，注意这里的图像块是圆形的！
        int d = u_max[v];
        //在坐标范围内挨个像素遍历，实际是一次遍历2个
        // 假设每次处理的两个点坐标，中心线下方为(x,y),中心线上方为(x,-y) 
        // 对于某次待处理的两个点：m_10 = Σ x*I(x,y) =  x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y))
        // 对于某次待处理的两个点：m_01 = Σ y*I(x,y) =  y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y))
        for (int u = -d; u <= d; ++u)
        {
            //得到需要进行加运算和减运算的像素灰度值
            //val_plus：在中心线下方x=u时的的像素灰度值，在下半圆上
            //val_minus：在中心线上方x=u时的像素灰度值，在上半圆上
            int val_plus = center[u + v*step], val_minus = center[u - v*step];
            //在v（y轴）上，2行所有像素灰度值之差
            v_sum += (val_plus - val_minus);
            //u轴（也就是x轴）方向上用u坐标加权和（u坐标也有正负符号），相当于同时计算两行
            m_10 += u * (val_plus + val_minus);
        }
        //将这一行上的和按照y坐标加权
        m_01 += v * v_sum;
    }

    //为了加快速度还使用了fastAtan2()函数，输出为[0,360)角度，精度为0.3°
    return fastAtan2((float)m_01, (float)m_10);
}

2.2.3 计算描述子computeDescriptors()

计算描述子前，先进行了GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);

(1)为了每次计算的描述子的踩点是一致的，先固定采样模板，也就是使用固定的采样像素对。代码在src/ORBextractor.cc文件中的 bit_pattern_31_ 变量, 其是 256*4 的数组。一对坐标4个元素，所以这种共256对。
(2)获得关键点角度，单纯的 BRIEF 描述子是不具备方向信息的，所以需要与关键点的角度结合起来。
(3)分成32次循环，每次循环对比8对像素值，共完成 32x8=256 次对比，代码中还使用位移操作来完成计算，这样加快了代码运行速率。

//注意这是一个不属于任何类的全局静态函数，static修饰符限定其只能够被本文件中的函数调用
/**
 * @brief 计算某层金字塔图像上特征点的描述子
 * 
 * @param[in] image                 某层金字塔图像
 * @param[in] keypoints             特征点vector容器
 * @param[out] descriptors          描述子
 * @param[in] pattern               计算描述子使用的固定随机点集
 */
static void computeDescriptors(const Mat& image, vector& keypoints, Mat& descriptors,
                               const vector& pattern)
{
    //清空保存描述子信息的容器
    descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);

    //开始遍历特征点
    for (size_t i = 0; i < keypoints.size(); i++)
        //计算这个特征点的描述子
        computeOrbDescriptor(keypoints[i],                 //要计算描述子的特征点
                             image,                     //以及其图像
                             &pattern[0],                 //随机点集的首地址
                             descriptors.ptr((int)i));    //提取出来的描述子的保存位置
}

/**
 * @brief 计算ORB特征点的描述子。注意这个是全局的静态函数，只能是在本文件内被调用
 * @param[in] kpt       特征点对象
 * @param[in] img       提取特征点的图像
 * @param[in] pattern   预定义好的采样模板
 * @param[out] desc     用作输出变量，保存计算好的描述子，维度为32*8 = 256 bit
 */
static void computeOrbDescriptor(const KeyPoint& kpt, const Mat& img, const Point* pattern, uchar* desc)
{
    //得到特征点的角度，用弧度制表示。其中kpt.angle是角度制，范围为[0,360)度
    float angle = (float)kpt.angle*factorPI;
    //计算这个角度的余弦值和正弦值
    float a = (float)cos(angle), b = (float)sin(angle);

    //获得图像中心指针
    const uchar* center = &img.at(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
    //获得图像的每行的字节数
    const int step = (int)img.step;

    //原始的BRIEF描述子没有方向不变性，通过加入关键点的方向来计算描述子，称之为Steer BRIEF，具有较好旋转不变特性
    //具体地，在计算的时候需要将这里选取的采样模板中点的x轴方向旋转到特征点的方向。
    //获得采样点中某个idx所对应的点的灰度值,这里旋转前坐标为(x,y), 旋转后坐标(x',y')，他们的变换关系:
    // x'= xcos(θ) - ysin(θ),  y'= xsin(θ) + ycos(θ)
    // 下面表示 y'* step + x'
    #define GET_VALUE(idx) center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + cvRound(pattern[idx].x*a - pattern[idx].y*b)]        
    
    //brief描述子由32*8位组成
    //其中每一位是来自于两个像素点灰度的直接比较，所以每比较出8bit结果，需要16个随机点，这也就是为什么pattern需要+=16的原因
    for (int i = 0; i < 32; ++i, pattern += 16)
    {
        
        int t0,     //参与比较的第1个特征点的灰度值
            t1,        //参与比较的第2个特征点的灰度值        
            val;    //描述子这个字节的比较结果，0或1
        
        t0 = GET_VALUE(0); t1 = GET_VALUE(1);
        val = t0 < t1;                            //描述子本字节的bit0
        t0 = GET_VALUE(2); t1 = GET_VALUE(3);
        val |= (t0 < t1) << 1;                    //描述子本字节的bit1
        t0 = GET_VALUE(4); t1 = GET_VALUE(5);
        val |= (t0 < t1) << 2;                    //描述子本字节的bit2
        t0 = GET_VALUE(6); t1 = GET_VALUE(7);
        val |= (t0 < t1) << 3;                    //描述子本字节的bit3
        t0 = GET_VALUE(8); t1 = GET_VALUE(9);
        val |= (t0 < t1) << 4;                    //描述子本字节的bit4
        t0 = GET_VALUE(10); t1 = GET_VALUE(11);
        val |= (t0 < t1) << 5;                    //描述子本字节的bit5
        t0 = GET_VALUE(12); t1 = GET_VALUE(13);
        val |= (t0 < t1) << 6;                    //描述子本字节的bit6
        t0 = GET_VALUE(14); t1 = GET_VALUE(15);
        val |= (t0 < t1) << 7;                    //描述子本字节的bit7

        //保存当前比较的出来的描述子的这个字节
        desc[i] = (uchar)val;
    }

    //为了避免和程序中的其他部分冲突在，在使用完成之后就取消这个宏定义
    #undef GET_VALUE
}

2.3 计算去畸变的图片边界ComputeImageBounds()

void Frame::ComputeImageBounds(const cv::Mat &imLeft)
{
    if(mDistCoef.at(0)!=0.0)
    {
        cv::Mat mat(4,2,CV_32F);
        mat.at(0,0)=0.0; mat.at(0,1)=0.0;
        mat.at(1,0)=imLeft.cols; mat.at(1,1)=0.0;
        mat.at(2,0)=0.0; mat.at(2,1)=imLeft.rows;
        mat.at(3,0)=imLeft.cols; mat.at(3,1)=imLeft.rows;

        mat=mat.reshape(2);
        cv::undistortPoints(mat,mat,static_cast(mpCamera)->toK(),mDistCoef,cv::Mat(),mK);
        mat=mat.reshape(1);

        // Undistort corners
        mnMinX = min(mat.at(0,0),mat.at(2,0));
        mnMaxX = max(mat.at(1,0),mat.at(3,0));
        mnMinY = min(mat.at(0,1),mat.at(1,1));
        mnMaxY = max(mat.at(2,1),mat.at(3,1));
    }
    else
    {
        mnMinX = 0.0f;
        mnMaxX = imLeft.cols;
        mnMinY = 0.0f;
        mnMaxY = imLeft.rows;
    }
}

2.4 双目(不同相机模型)匹配并恢复深度信息ComputeStereoFishEyeMatches()

这一部分没有利用极线搜索+块匹配的方式获取左右目之间的匹配关系，代码中首先把左右image中靠后的，也就是处于lapping area的特征点的像素坐标和描述子找出来。然后根据Nleft/Nright初始化存放匹配上的特征点的容器。monoLeft/monoRight是在ORBextractor::operator()中获取的，它记录着处于和不处于lapping area的特征点的分界位置，见2.2。

void Frame::ComputeStereoFishEyeMatches() {
    //Speed it up by matching keypoints in the lapping area
    //把处于lapping area的特征点和它们的描述子找出来，对应着mvKeys容器中，monoLeft以后的那些特征点
    vector stereoLeft(mvKeys.begin() + monoLeft, mvKeys.end());
    vector stereoRight(mvKeysRight.begin() + monoRight, mvKeysRight.end());
    cv::Mat stereoDescLeft = mDescriptors.rowRange(monoLeft, mDescriptors.rows);
    cv::Mat stereoDescRight = mDescriptorsRight.rowRange(monoRight, mDescriptorsRight.rows);

    //利用左右目最大特征点数初始化用于存储匹配的特征点的容器
    mvLeftToRightMatch = vector(Nleft,-1);
    mvRightToLeftMatch = vector(Nright,-1);
    mvDepth = vector(Nleft,-1.0f);
    mvuRight = vector(Nleft,-1);
    mvStereo3Dpoints = vector(Nleft);
    mnCloseMPs = 0;

然后在左右image进行brute force匹配，注意，左右image匹配上的特征点是可能在不同图层上的。

    //Perform a brute force between Keypoint in the left and right image
    vector> matches;
    BFmatcher.knnMatch(stereoDescLeft,stereoDescRight,matches,2);

遍历每一个匹配结果，其中一个vector保存着在另一个照片中与当前照片某个特征点相匹配的所有的特征点，并且它是按照描述子距离从小到大排序，因此头2个一定是距离最近的备选匹配点，如果第1个备选匹配点的描述子距离<0.7*第2个备选匹配点的描述子距离，则接受第1个匹配点，这就是Lowe's ratio。因为对于错误匹配，由于特征空间的高维性，相似的距离可能有大量其他的错误匹配，从而它的ratio值比较高，因此ratio越小，第1个匹配点就越可能是正确的匹配。

    //Check matches using Lowe's ratio
    for(vector>::iterator it = matches.begin(); it != matches.end(); ++it){
        if((*it).size() >= 2 && (*it)[0].distance < (*it)[1].distance * 0.7){

            //For every good match, check parallax and reprojection error to discard spurious matches
            Eigen::Vector3f p3D; //camera系下的3D坐标
            descMatches++;
            
            //获取两个特征点各自所在图层的缩放系数
            float sigma1 = mvLevelSigma2[mvKeys[(*it)[0].queryIdx + monoLeft].octave]
            float sigma2 = mvLevelSigma2[mvKeysRight[(*it)[0].trainIdx + monoRight].octave];
            
            //三角化特征点求出深度
            float depth = static_cast(mpCamera)->TriangulateMatches(mpCamera2,mvKeys[(*it)[0].queryIdx + monoLeft],mvKeysRight[(*it)[0].trainIdx + monoRight],mRlr,mtlr,sigma1,sigma2,p3D);
            
            //如果深度值大于阈值，则保留相关数据和匹配的索引关系，并认为是一次正确的匹配
            if(depth > 0.0001f){
                mvLeftToRightMatch[(*it)[0].queryIdx + monoLeft] = (*it)[0].trainIdx + monoRight;
                mvRightToLeftMatch[(*it)[0].trainIdx + monoRight] = (*it)[0].queryIdx + monoLeft;
                mvStereo3Dpoints[(*it)[0].queryIdx + monoLeft] = p3D;
                mvDepth[(*it)[0].queryIdx + monoLeft] = depth;
                nMatches++;
            }
        }
    }
}

2.5 双目(相同相机模型)匹配并恢复深度信息ComputeStereoMatches()

2.4针对着左右相机模型不同的情况，如果相同，则调用的是这个函数，并且在调用这个函数之前就应该进行去畸变，这一部分是和ORB-SLAM2一致。
参考链接：https://blog.csdn.net/ainitutu/article/details/109232070 

这部分的主要思想是左右两个image上对应的匹配点行数是差不多的，那么遍历每一个左特征点，在右image上找到对应那几行中且在视差范围内的那几列的备选匹配点，然后在备选匹配点中找到描述子距离最小的那个。虽然此时右目的这个匹配点是所有点中与当前左目特征点最像的那一个，但不一定是严格对应的那一个，所以再进行块匹配提高精度。在左特征点找一个方块，右特征点找一个方块，依次滑动右方块，找到两个方块最像时右方块的位置。最后，亚像素插值获得精确的右匹配点坐标，并恢复该特征点的深度。

 * 为左图的每一个特征点在右图中找到匹配点
 * 根据基线(有冗余范围)上描述子距离找到匹配, 再进行SAD精确定位
 * 这里所说的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(N,-1.0f);
    mvDepth = vector(N,-1.0f);

    // orb特征相似度阈值  -> mean ～= (max  + min) / 2 =75;
    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 > vRowIndices(nRows, vector());
    for(int i=0; i

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也跟着变化
        // r= ２＊１、２＊0.83^1、２*0.83^2...
        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);
    }

Step 2 -> 3. 粗匹配 + 精匹配：

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

    // Stereo baseline multiplied by fx.
    ///float mbf; baseline x fx
    

    // Stereo baseline in meters.
    // float mb; 相机的基线长度,单位为米 ,mb = mbf/fx;(z=f*b/d:d视差，ｚ深度）
    
    const float minZ = mb;//minZ深度
    const float minD = 0;             
    const float maxD = mbf/minZ; //mbf:相机的基线长度 * 相机的焦距

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

    // 为左图每一个特征点il，在右图搜索最相似的特征点ir
    for(int iL=0; iL &vCandidates = vRowIndices[vL];
        if(vCandidates.empty()) continue;

        // 计算理论上的最佳搜索范围
        const float minU = uL-maxD;//改行中搜索最小列
        const float maxU = uL-minD;//改行中搜索最大列，就是其ｖｌ=uL=ｍａｘU
        
        // 最大搜索范围小于0，说明无匹配点
        if(maxU<0) continue;

        // 初始化最佳相似度，用最大相似度，以及最佳匹配点索引
        int bestDist = ORBmatcher::TH_HIGH;//100;
        size_t bestIdxR = 0;
        //(il:作图特征点编号)左目摄像头和右目摄像头特征点对应的描述子 mDescriptors, mDescriptorsRight;
        const cv::Mat &dL = mDescriptors.row(iL);//dL用来计算描述子的汉明距离；但描述子的row表示什么？
        
        // Step2. 粗配准. 左图特征点il与右图中的可能的匹配点进行逐个比较,得到最相似匹配点的相似度和索引
        for(size_t iC=0; iClevelL+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)
                //初始bestDist=??
                //初始bestIdxR=0
                if( distmvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);
            // convertTo()函数负责转换数据类型不同的Mat，即可以将类似float型的Mat转换到imwrite()函数能够接受的类型。
            IL.convertTo(IL,CV_32F);
            
            // 图像块均值归一化，降低亮度变化对相似度计算的影响
            IL = IL - IL.at(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 vDists;
            vDists.resize(2*L+1); 

            // 计算滑动窗口滑动范围的边界，因为是块匹配，还要算上图像块的尺寸
            // 列方向起点 iniu = r0 + 最大窗口滑动范围 - 图像块尺寸
            // 列方向终点 eniu = r0 + 最大窗口滑动范围 + 图像块尺寸 + 1
            // 此次 + 1 和下面的提取图像块是列坐标+1是一样的，保证提取的图像块的宽是2 * w + 1
            const float iniu = scaleduR0+L-w;// scaleduR0:右图粗匹配到的金字塔尺度的特征点坐标ｘ
            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(w,w) * cv::Mat::ones(IR.rows,IR.cols,CV_32F);
                
                // sad 计算
                float dist = cv::norm(IL,IR,cv::NORM_L1);

                // 统计最小sad和偏移量
                if(dist vDists;vDists.resize(2*L+1)=11;
                vDists[L+incR] = dist;      
            }

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

Step 4. 亚像素插值, 使用最佳匹配点及其左右相邻点构成抛物线

            // 使用3点拟合抛物线的方式，用极小值代替之前计算的最优是差值
            //    \                 / <- 由视差为14，15，16的相似度拟合的抛物线
            //      .             .(16)
            //         .14     .(15) <- int/uchar最佳视差值
            //              . 
            //           （14.5）<- 真实的视差值
            //   deltaR = 15.5 - 16 = -0.5
            // 公式参考opencv sgbm源码中的亚像素插值公式
            // 或论文<> 公式7

            const float dist1 = vDists[L+bestincR-1];    //bestincR:列坐标偏移量
            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);
            // disparity:求得的视差值
            float disparity = (uL-bestuR);
            if(disparity>=minD && disparity

Step 5. 最优视差值/深度选择.

                // 根据视差值计算深度信息
                // 保存最相似点的列坐标(x)信息
                // 保存归一化sad最小相似度
                mvDepth[iL]=mbf/disparity;
                mvuRight[iL] = bestuR;//根据亚像素精度偏移量delta调整最佳匹配索引,视差disparity = (uL-bestuR);
                // vDistIdx 二维数组存放 ＳＡＤ计算的最小滑块匹配值和左图这个特征的编号
                vDistIdx.push_back(pair(bestDist,iL));
        } //end if   
    }//遍历左图每个特征点

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

2.6 双目(RGBD)匹配并恢复深度信息ComputeStereoMatches()

RGBD模式下，深度值已知，所以这部分就很简单了。

void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth)
{
    mvuRight = vector(N,-1);
    mvDepth = vector(N,-1);

    for(int i=0; i(v,u);

        if(d>0)
        {
            mvDepth[i] = d;
            mvuRight[i] = kpU.pt.x-mbf/d;
        }
    }
}

2.7 把特征点放到格子里AssignFeaturesToGrid()

这一部分内容与ORB-SLAM2也有一些区别，主要增加了对右image的考虑。

void Frame::AssignFeaturesToGrid()
{
    // Fill matrix with points
    //给每一个格子先开辟一个内存空间，ncells是总格子数，N/(nCells)是平均每个格子所拥有的特征点数，每一个格子开辟0.5倍这个数的空间
    const int nCells = FRAME_GRID_COLS*FRAME_GRID_ROWS;
    int nReserve = 0.5f*N/(nCells);

    //接下来给每一个格子开辟内存空间
    for(unsigned int i=0; i= Nleft对应stereo中右image的特征点
        const cv::KeyPoint &kp = (Nleft == -1) ? mvKeysUn[i] : (i < Nleft) ? mvKeys[i] : mvKeysRight[i - Nleft];

        int nGridPosX, nGridPosY;
        if(PosInGrid(kp,nGridPosX,nGridPosY)){
            if(Nleft == -1 || i < Nleft) mGrid[nGridPosX][nGridPosY].push_back(i);
            else mGridRight[nGridPosX][nGridPosY].push_back(i - Nleft);
        }
    }
}

2.8 左目特征点去畸变UndistortKeyPoints()

这个函数需要注意一点，它只计算了左目去畸变的特征点像素坐标。

void Frame::UndistortKeyPoints()
{
    // Step 1 如果第一个畸变参数为0，不需要矫正。第一个畸变参数k1是最重要的，一般不为0，为0的话，说明畸变参数都是0
    //变量mDistCoef中存储了opencv指定格式的去畸变参数，格式为：(k1,k2,p1,p2,k3)
    if(mDistCoef.at(0)==0.0)
    {
        mvKeysUn=mvKeys;
        return;
    }


    // Step 2 如果畸变参数不为0，用OpenCV函数进行畸变矫正
    // Fill matrix with points
    // N为提取的特征点数量，为满足OpenCV函数输入要求，将N个特征点保存在N*2的矩阵中
    cv::Mat mat(N,2,CV_32F);
    //遍历每个特征点，并将它们的坐标保存到矩阵中
    for(int i=0; i(i,0)=mvKeys[i].pt.x;
        mat.at(i,1)=mvKeys[i].pt.y;
    }

    // Undistort points
    // 函数reshape(int cn,int rows=0) 其中cn为更改后的通道数，rows=0表示这个行将保持原来的参数不变
    //为了能够直接调用opencv的函数来去畸变，需要先将矩阵调整为2通道（对应坐标x,y） 
    mat=mat.reshape(2);
    cv::undistortPoints(    
        mat,                //输入的特征点坐标
        mat,                //输出的校正后的特征点坐标覆盖原矩阵
        mK,                    //相机的内参数矩阵
        mDistCoef,            //相机畸变参数矩阵
        cv::Mat(),            //一个空矩阵，对应为函数原型中的R
        mK);                 //新内参数矩阵，对应为函数原型中的P
    
    //调整回只有一个通道，回归我们正常的处理方式
    mat=mat.reshape(1);

    // Fill undistorted keypoint vector
    // Step 存储校正后的特征点
    mvKeysUn.resize(N);
    //遍历每一个特征点
    for(int i=0; i(i,0);
        kp.pt.y=mat.at(i,1);
        mvKeysUn[i]=kp;
    }
}