ORB-SLAM2中四叉樹管理特征點

當從圖像金字塔中的每一層圖像上提取特征點之后，都要先用四叉樹技術對這些特征點進行管理

//該類中定義了四叉樹創建的函數以及樹中結點的屬性
//bool bNoMore： 根據該結點中被分配的特征點的數目來決定是否繼續對其進行分割
//DivisionNode()：實現如何對一個結點進行分割
//vKeys：用來存儲被分配到該結點區域內的所有特征點
//UL, UR, BL, BR：四個點定義了一個結點的區域
//lit:list的迭代器，遍歷所有生成的節點
class ExtractorNode
{
public:///用初始化列表來初始化本類內的成員變量ExtractorNode():bNoMore(false){}void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4);std::vector<cv::KeyPoint> vKeys;cv::Point2i UL, UR, BL, BR;std::list<ExtractorNode>::iterator lit;bool bNoMore;
};

下面的函數實現利用四叉樹來管理從金字塔中的每一層圖像上提取的特征點

//vToDistributeKeys變量中存儲的是從金字塔中某一層圖像上提取的特征點
//minX, maxX, minY, maxY：是該層圖像去除了邊界的區域
//N: mnFeaturesPerLevel[i]表示該層圖像上應該提取的特征點的個數
//level: 該圖像處在金字塔上的層數
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)
{//常用的相機kinect v1的分辨率是：640*480 kinect v2的分辨率是：1920*1080//為了盡量使得每一個結點的區域形狀接近正方形所以圖像的長寬比決定了四叉樹根節點的數目//如果使用kinect v1那么只有一個根結點，如果使用kinect v2那么就會有兩個根結點const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));//hX變量可以理解為一個根節點所占的寬度  const float hX = static_cast<float>(maxX-minX)/nIni;//lNodes中存儲生成的樹結點list<ExtractorNode> lNodes;//vpIniNodes變量中存儲的是結點的地址vector<ExtractorNode*> vpIniNodes;//vpIniNodes的大小先設置成根結點的個數vpIniNodes.resize(nIni);for(int i=0; i<nIni; i++){ExtractorNode ni;//四叉樹是每次根據特定條件將一個結點分成四等分，四個區域左上(UL)，右上(UR)， //左下(BL)，右下(BR)//左上角位置坐標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);//vKeys的大小為在上面的這個根節點范圍內總共提取的特征點的個數ni.vKeys.reserve(vToDistributeKeys.size());//將創建的根節點插入到list lNodes中lNodes.push_back(ni);將lNodes變量中的最后存儲的那個結點的地址存儲到vector變量vpIniNodes中//暫時還不知道這個變量做何用//看都了吧vpIniNodes總是把最后插入到lNodes中的結點的地址拿走，然后要為//該結點的vKeys成員變量內部添加屬于該結點的特征點。vpIniNodes[i] = &lNodes.back();}//Associate points to childs//要一直記得vToDistributeKeys變量中存儲的是該層圖像中提取的特征點//遍歷在該層圖像上提取的所有特征點for(size_t i=0;i<vToDistributeKeys.size();i++){const cv::KeyPoint &kp = vToDistributeKeys[i];//將所有提取的特征點根據坐標位置將其分配到對應的根節點中去//如果使用kinect b=v1那么所有的kp.pt.x都小于hX,所以所有的特征點都被分配到//vpIniNodes的第0個元素中存儲的結點指針所指向的空間中去了。//到這里明白了這個四叉樹的玩法了//定義一個list變量，用來存儲生成的樹節點本身//定義一個vector變量，用來存儲結點的指針，這個指針可以指向該結點區域被分配的特征點的內存//list是一個雙向鏈表容器，可高效地進行插入刪除元素//vector是將元素置于一個動態數組中，vector可以隨機存取元素，在頭尾插入數據快，但是從中            //間插入數據很慢//正是利用了list和vector的特點，使得我們即可以快速高效的插入刪除結點，又可以隨機的存取//被分配到某一個結點區域的的特征點//如何將這個list和vector聯系起來共同維護這個四叉樹呢？vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);}list<ExtractorNode>::iterator lit = lNodes.begin();//遍歷已經生成的所有節點while(lit!=lNodes.end()){//如果判斷在一個結點里面只有一個特征點if(lit->vKeys.size()==1){//將該結點的bNoMore屬性設置為true，表示不再對這個結點進行分割lit->bNoMore=true;lit++;}//如果判斷這個結點中沒有被分配到任何的特征點那么就將這個結點刪除else if(lit->vKeys.empty())lit = lNodes.erase(lit);elselit++;}//lNodes中的結點和 vpIniNodes中的結點指針是同步的，只有在 vpIniNodes中存儲的結點中存儲了 //特征點，才能根據特征點的數目來決定如何處理這個結點//那如果在lNodes中刪除一個沒有特征點的結點，那么在 vpIniNodes中對應的這個地址也會被銷毀嗎？bool bFinish = false;int iteration = 0;vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode;vSizeAndPointerToNode.reserve(lNodes.size()*4);while(!bFinish){iteration++;//lNodes中已經存儲的結點的數目int prevSize = lNodes.size();lit = lNodes.begin();int nToExpand = 0;vSizeAndPointerToNode.clear();while(lit!=lNodes.end()){///如果結點內被分配的特征點的數目只有1個則不繼續分割這個結點if(lit->bNoMore){// If node only contains one point do not subdivide and continuelit++;continue;}else{// 如果結點中被分配到的特征點數大于1則要繼續分割ExtractorNode n1,n2,n3,n4;//在下面在介紹這個函數//概括來說就是將上面這個結點分成了四個結點，并且已經完成了特征點的分配，以及特征//個數的檢測設定好每個節點的bNoMore的值lit->DivideNode(n1,n2,n3,n4);// Add childs if they contain pointsif(n1.vKeys.size()>0){//如果新分割出來的第一個結點中被分配的特征點的個數大于0那么就將這個結點//插入到list的頭部lNodes.push_front(n1);//如果這個心結點中被分配的特征點的個數大于1，那么接下來要被分割的結點的數目//就得加1了                    if(n1.vKeys.size()>1){nToExpand++;//變量vSizeAndPointerToNode中存儲的是每一個結點的地址以及該結點中被分配到的特征點的個數。                    vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));lNodes.front().lit = lNodes.begin();}}//對新分配出的第二個結點進行同上面相同的測試和操作if(n2.vKeys.size()>0){    //在list的頭部插入元素lNodes.push_front(n2);if(n2.vKeys.size()>1){nToExpand++;vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));//每插入一個結點就要更新list的開始結點的位置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;}//如果現在生成的結點全部進行分割后生成的結點滿足大于需求的特征點的數目，但是不繼續分割又//不能滿足大于N的要求時//這里為什么是乘以三，這里也正好印證了上面所說的當一個結點被分割成四個新的結點時，//這個結點時要被刪除的，其實總的結點時增加了三個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 pointsif(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);//如果多有的結點還沒有被分割完就已經達到了大于N的要求那么就直接跳出循環if((int)lNodes.size()>=N)break;}if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)bFinish = true;}}}// Retain the best point in each nodevector<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;//在每個結點中找到那個最強壯的特征點進行保存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;
}

經過上面的操作，我們已經將圖像金字塔中的某一層圖像上提取的特征點優化完畢了。

//再來看看結點時如何被分割成四個新的結點的
//這個成員函數的參數是四個引用，所以函數返回值是void，C++中的引用就好比與C語言中的指針
void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)
{//static_cast就相當于C語言中的強制類型轉換//在分割結點之前要先計算出每一個新結點的四個角點坐標//halfX和halfY分別是已創建結點的x方向的和y方向的中間位置const int halfX = ceil(static_cast<float>(UR.x-UL.x)/2);const int halfY = ceil(static_cast<float>(BR.y-UL.y)/2);//設定四個子結點的邊界n1.UL = UL;n1.UR = cv::Point2i(UL.x+halfX,UL.y);n1.BL = cv::Point2i(UL.x,UL.y+halfY);n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY);//將每個新結點的用來存儲特征點的向量的capacity設置為母結點中所有的特征點的個數n1.vKeys.reserve(vKeys.size());n2.UL = n1.UR;n2.UR = UR;n2.BL = n1.BR;n2.BR = cv::Point2i(UR.x,UL.y+halfY);n2.vKeys.reserve(vKeys.size());n3.UL = n1.BL;n3.UR = n1.BR;n3.BL = BL;n3.BR = cv::Point2i(n1.BR.x,BL.y);n3.vKeys.reserve(vKeys.size());n4.UL = n3.UR;n4.UR = n2.BR;n4.BL = n3.BR;n4.BR = BR;n4.vKeys.reserve(vKeys.size());//Associate points to childs//根據特征點的坐標來將特征點分配到不同的新結點區域for(size_t i=0;i<vKeys.size();i++){const cv::KeyPoint &kp = vKeys[i];if(kp.pt.x<n1.UR.x){if(kp.pt.y<n1.BR.y)n1.vKeys.push_back(kp);elsen3.vKeys.push_back(kp);}else if(kp.pt.y<n1.BR.y)n2.vKeys.push_back(kp);elsen4.vKeys.push_back(kp);}//最后根據每個結點中分得的特征點的數目來設定bNoMore變量的真假if(n1.vKeys.size()==1)n1.bNoMore = true;if(n2.vKeys.size()==1)n2.bNoMore = true;if(n3.vKeys.size()==1)n3.bNoMore = true;if(n4.vKeys.size()==1)n4.bNoMore = true;}

接下來捋一捋四叉樹的數據模型

假設一個四叉樹的形狀是這樣的，看看他在內存中是如何存儲和管理結點的

在ORB-SLAM中是定義了一個list用來存儲四叉樹中的結點 std::vector<ExtractNode> lNodes;

如果一個結點滿足被分割的條件那么將他新分割出來的四個結點依次從頭部插入到list，每插入一個新的結點就讓list的迭代器指向list中的第一個元素，四個新結點插入完畢之后將母結點刪除

然后是從list頭部開始依次遍歷所有結點，判斷哪個節點要被繼續分割，如果需要繼續分割則按照上面的方法完成新結點的存儲和母結點的刪除，因為每插入一個結點都會讓迭代器指向新插入的結點，所以每次都是從list的頭部開始遍歷，所以剛被分割出來的結點如果滿足分割條件會被緊接著繼續分割，而最早生成的結點要越晚被分割。

接下來談一談為什么要將四叉樹應用到ORB-SLAM中，聽別人說是為了管理圖像中提取的特征點，那這個四叉樹到底在特征點的管理中起到了什么作用呢？

記得很清楚的一點是在上面的程序中，有一個環節是將創建好的結點按照結點中包含的特征點的個數從小到大來對結點進行排序，然后是優先對那些包含特征點相對較少的結點進行分割，因為結點中包含的特征點少，分割的次數也就少了，并且倘若還沒有遍歷完所有的結點得到的特征點的個數就已經達到了每層圖像提取特征點的要求，那么就不必對那些包含特征點多的結點進行分割了，一個是分割起來費勁，再者，這個結點中包含比同等層次的結點要多的特征點，說明這里面的特征點有“集會滋事”的嫌疑，畢竟我們希望最終得到的特征點盡可能的均勻的分布在圖像中。

接下來，就要對從上面每個結點中挑選出response最大的作為這個結點的代表保留下來，我想的這樣一來，萬一留下的特征點的數目沒有達標怎么辦呢？欲知后事如何，請聽下次分曉。