ORB-SLAM（一）：关于ORB-SLAM中四叉树的使用

ORB-SLAM中使用四叉树来快速筛选特征点，筛选的目的是非极大值抑制，取局部特征点邻域中FAST角点相应值最大的点，而如何搜索到这些扎堆的的点，则采用的是四叉树的分快思想，递归找到成群的点，并从中找到相应值最大的点。

算法的核心流程：

第一部分：

1. 输入图像未分的关键点
2. 根据图像区域构造初始的根结点，每个根结点包含图像的一个区域，每个根结点同样包括4个子结点
3. 将未分的所有关键点分配给2中构造的根结点，这样每个根节点都包含了其所负责区域内的所有关键点。
4. 根结点构成一个根结点list，代码中是lNodes用来更新与存储所有的根结点。

第二部分

• while(INodes中还有可分的结点)：
  • for 每一个根结点 in INodes：
  1. 判断当前根结点是否可分，可分的意思是，它包含的关键点能够继续分配到其所属的四个子结点所在区域中（左上，右上，左下，右下）
  2. 如果可分，将分出来的子结点作为新的根结点放入INodes的前部，e.g. lNodes.push_front(ni)，然后将原先的根结点从列表中删除，由于新加入的结点是从列表头加入的，不会影响这次的循环，该次循环只会处理当前级别的根结点。
  3. 当所有结点不可分，或者结点已经超过需要的点时，跳出循环

第三部分
当所有根结点都不可分时，不可分的意思是，该结点包含的关键点，没法再往更小的区域上分配了，那么将不可分的根节点中包含关键点大于1个的结点拎出来，这些关键点在很小的区域上扎堆了，因此算法从中选出角点响应值最大的关键点作为该根结点的关键点。
最后所有结点都只包含1个关键点，扎堆的关键点就都被处理了。

个人感觉，用四叉树来搜索扎堆的关键点很快，算法规范且有章法。

下面是代码的注释：

vector<cv::KeyPoint> ORBextractor::DistributeOctTree(const vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
                                       const int &maxX, const int &minY, const int &maxY, const int &N, const int &level)
{
    // Compute how many initial nodes
    // 首先计算根结点的个数
    // 默认图像 宽>长
    const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));

    const float hX = static_cast<float>(maxX-minX)/nIni;

    list<ExtractorNode> lNodes;

    vector<ExtractorNode*> vpIniNodes;
    vpIniNodes.resize(nIni);

    // 根据原始根结点数量,设置其4个对应的子结点,并将其加入到根节点的队列中
    for(int i=0; i<nIni; i++)
    {
        ExtractorNode ni;
        ni.UL = cv::Point2i(hX*static_cast<float>(i),0);
        ni.UR = cv::Point2i(hX*static_cast<float>(i+1),0);
        ni.BL = cv::Point2i(ni.UL.x,maxY-minY);
        ni.BR = cv::Point2i(ni.UR.x,maxY-minY);
        ni.vKeys.reserve(vToDistributeKeys.size());

        lNodes.push_back(ni);
        vpIniNodes[i] = &lNodes.back();
    }

    // 将所有的特征点加入到初始的对应的根结点中
    // 其实这里可以直接用lNodes进行寻址,用指针容器是否要快一点
    for(size_t i=0;i<vToDistributeKeys.size();i++)
    {
        const cv::KeyPoint &kp = vToDistributeKeys[i];

        // 对于宽/长没有超过1.5的图像,所有的特征点都归入了唯一的根结点
        vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
    }

    list<ExtractorNode>::iterator lit = lNodes.begin();

    // 检查结点中若有空节点则去掉,若结点包含的特征点为1则表示其不再分
    while(lit!=lNodes.end())
    {
        if(lit->vKeys.size()==1)
        {
            lit->bNoMore=true;
            lit++;
        }
        else if(lit->vKeys.empty())
            lit = lNodes.erase(lit);
        else
            lit++;
    }

    bool bFinish = false;

    int iteration = 0;

    vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode;
    vSizeAndPointerToNode.reserve(lNodes.size()*4);

    // 开始循环,循环的目的是按照四叉树细分每个结点,直到每个结点不可分为止,不可分包含两种情况,结点所含特征点为1或>1,之所以要用四叉树,
    // 就是要快速找到扎堆的点并取其最大响应值的点作为最终的特征点
    while(!bFinish)
    {
        iteration++;

        int prevSize = lNodes.size();

        lit = lNodes.begin();

        // nToExpand代表还可分的结点个数
        int nToExpand = 0;

        vSizeAndPointerToNode.clear();

        // 一次循环下来,将当前lNode中的根结点全部细分成子结点,并将子结点加入到lNode的前部且不会被遍历到,删除对应的根结点
        // 注意一次遍历,只会处理根结点,新加入的子结点不会被遍历到
        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;
                lit->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)
                    {
                        nToExpand++;
                        // 若子结点中特征点超过1,则将其指针记录在vSizeAndPointerToNode之中
                        vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
                        // 当前 lNodes.front() = n1
                        // n1.lit = lNode.begin(),也即它自己的迭代器指向它在lNode中的位置
                        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();
                    }
                }

                // 分完过后,删除当前根结点
                lit=lNodes.erase(lit);
                continue;
            }
        }       

        // Finish if there are more nodes than required features
        // or all nodes contain just one point
        if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
        {
            bFinish = true;
        }
        // 对于头一次从根结点,假设根结点只有1个,那么分出来的子结点只有4个,那么下面的的条件是进入不了的
        // 当分的差不多时,只有个别结点未分时,进入以下逻辑
        else if(((int)lNodes.size()+nToExpand*3)>N)
        {

            while(!bFinish)
            {

                prevSize = lNodes.size();

                vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
                vSizeAndPointerToNode.clear();

                // 将元素按升序排列,这里的假设是包含的特征点少的结点往往已经不可分了
                sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
                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.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);

                    if((int)lNodes.size()>=N)
                        break;
                }

                if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
                    bFinish = true;

            }
        }
    }

    // Retain the best point in each node
    vector<cv::KeyPoint> vResultKeys;
    vResultKeys.reserve(nfeatures);
    for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
    {
        vector<cv::KeyPoint> &vNodeKeys = lit->vKeys;
        cv::KeyPoint* pKP = &vNodeKeys[0];
        float maxResponse = pKP->response;

        // 如果vNodeKeys中只有1个关键点,则直接将其加入到最终结果点中
        // 如果最终的结点中有多个点,那么只有最大相应值的结点会被留下了
        for(size_t k=1;k<vNodeKeys.size();k++)
        {
            if(vNodeKeys[k].response>maxResponse)
            {
                pKP = &vNodeKeys[k];
                maxResponse = vNodeKeys[k].response;
            }
        }

        vResultKeys.push_back(*pKP);
    }

    return vResultKeys;
}