ORB - SLAM (1): ORB - SLAM 의 사지 수 사용

ORB - SLAM 에 서 는 사지 수 를 사용 하여 특징 점 을 신속하게 선별 합 니 다. 선별 의 목적 은 비 극 대 치 억제 입 니 다. 국부 특징 점 인접 지역 에서 FAST 각 점 의 해당 치가 가장 큰 점 을 취하 고 이런 쌓 인 점 을 어떻게 검색 하 는 지 는 사지 수의 분 속 사상 을 사용 하여 무 리 를 이 루 는 점 을 찾 고 그 중에서 해당 치가 가장 큰 점 을 찾 습 니 다.
알고리즘 의 핵심 프로 세 스:
첫 번 째 부분:

이미지 에 구분 되 지 않 은 관건 을 입력 하 십시오

이미지 구역 구조의 초기 뿌리 결점 에 따라 각 노드 는 이미지 의 한 구역 을 포함 하고 각 노드 는 4 개의 노드

를 포함한다.

구분 되 지 않 은 모든 관건 점 을 2 중 구조의 뿌리 결점 에 분배 하면 모든 노드 는 그 가 담당 하 는 구역 내의 모든 관건 점 을 포함한다.

루트 노드 는 하나의 노드 list 를 구성 하고 코드 는 lNodes 로 모든 루트 노드 를 업데이트 하고 저장 합 니 다.

제2 부분

while (INodes 에 나 눌 수 있 는 결점 이 있 음):

for 각 노드 in INodes:

현재 뿌리 결점 을 나 눌 수 있 는 지 여 부 를 판단 한다. 나 눌 수 있 는 관건 은 그 가 속 한 네 개의 자 결점 이 있 는 구역 (왼쪽 위, 오른쪽 위, 왼쪽 아래, 오른쪽 아래)

에 계속 분배 할 수 있다 는 뜻 이다.

나 눌 수 있 으 면 분 리 된 자 결점 을 새로운 뿌리 결점 으로 INodes 의 앞부분 에 넣 습 니 다. 예 를 들 어 lNodes. pushfront (ni), 그리고 원래 의 루트 노드 를 목록 에서 삭제 합 니 다. 새로 추 가 된 노드 는 목록 헤더 에서 추 가 된 것 이기 때문에 이번 순환 에 영향 을 주지 않 습 니 다. 이번 순환 은 현재 단계 의 루트 노드 만 처리 합 니 다.

모든 결점 을 나 눌 수 없 거나 결점 이 필요 한 점 을 초과 할 때 순환

세 번 째 부분 에서 모든 뿌리 결산 점 을 나 눌 수 없 을 때 나 눌 수 없다 는 뜻 은 이 결산 점 에 포 함 된 관건 점 을 더 작은 구역 에 분배 할 수 없다 는 것 이다. 그러면 나 눌 수 없 는 뿌리 노드 에 포 함 된 관건 점 이 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;
}