ORB-SLAM中四叉樹管理角點
- 2019 年 11 月 13 日
- 筆記
四叉樹索引的基本思想是將地理空間遞歸劃分為不同層次的樹結構。它將已知範圍的空間等分成四個相等的子空間，如此遞歸下去，直至樹的層次達到一定深度或者滿足某種要求後停止分割。
四叉樹對於區域查詢，效率比較高。但如果空間對象分布不均勻，隨著地理空間對象的不斷插入，四叉樹的層次會不斷地加深，將形成一棵嚴重不平衡的四叉樹，那麼每次查詢的深度將大大的增多，從而導致查詢效率的急劇下降。
四叉樹的定義
四元樹又稱四叉樹是一種樹狀數據結構，在每一個節點上會有四個子區塊。四元樹常應用於二維空間數據的分析與分類。它將數據區分成為四個象限。數據範圍可以是方形或矩形或其他任意形狀。
這種數據結構是由 拉斐爾·芬科爾(Raphael Finkel) 與 J. L. Bentley 在1974年發展出來 。四叉樹是在二維圖片中定位像素的唯一適合的演算法。因為二維空間（圖經常被描述的方式）中，平面像素可以重複的被分為四部分，樹的深度由圖片、電腦記憶體和圖形的複雜度決定。
四叉樹的特點
（1）可分解成為各自的區塊
（2）每個區塊都有節點容量。當節點達到最大容量時，節點分裂
（3）樹狀數據結構構造四元樹法加以區分
ORB_SLAM中的四叉樹
以上是理論部分，接下來主要理解在ORB_SLAM程式碼實現中，是如何實現四叉樹管理特徵點的從理論到實踐部分。在程式中，定義了一個平面區域的四個子區域索引號，左上為第一象限0，右上為第二象限1，左下為第三象限2，右下為第四象限3。一個矩形區域的象限劃分：
UL(0) | UR(1)
———-|———–
BL(2) | BR(3)
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;
//list的迭代器，遍歷所有生成的節點
bool bNoMore;
//根據該節點中被分配的特徵點的數目來決定是否對其繼續分割
};
演算法實現的步驟與程式碼中的細節
（1）輸入影像原始提取到的FAST關鍵點
（2）根據影像區域劃分初始的根節點，每個根節點包含影像的一個區域，每個根節點同樣包含了四個子節點
為了盡量使得每一個結點的區域形狀接近正方形所以影像的長寬比決定了四叉樹根節點的數目 如果使用640*480影像，那麼只有一個根結點，如果使用1920*1080影像，那麼就會有兩個根結點，雖然有影像邊界的擴大處理，但是不影響根節點的數量：
640/480=1.333 ===1
1920/1080=1.777777777====2
（3）將未劃分的所有關鍵點根據區域位置分配給步驟2中構造的根節點，這樣每個根節點都包含了所負責區域內的所有關鍵點。
（4）根節點構成一個根節點list，ORB_SLAM 的程式碼中是list<ExtractorNode> lNodes用來更新與存儲所有的根節點。list是一個雙向鏈表容器，用來存儲生成的樹節點本身，可高效地進行插入刪除元素，並且定義一個
vector<ExtractorNode*> vpIniNodes 變數，用來存儲結點的指針，這個指針可以指向該結點區域被分配的特徵點的記憶體。
步驟（3）細分
for( INodes每一個根節點 )
1，判斷當前根節點是否可再分，是否可以再繼續分配到所屬的四個子節點所在區域。
2，如果可分，將分出來的子節點作為新的根節點放在INodes的前部，e.g. lNodes.push_front(ni)，然後將原先的根結點從列表中刪除，由於新加入的結點是從列表頭加入的，不會影響這次的循環，該次循環只會處理當前級別的根結點。
3，當所有結點不可分時，將該結點的bNoMore屬性設置為true，表示不再對這個結點進行分割
總之目的是想讓角點在影像上的分布更均勻，提高運行效率。一張示意圖解釋概率四叉樹在其中的作用
從這張圖片上可以看出，左圖內紅色框框內的UR和BR都只有一個角點，而UL，BL有多個角點扎堆，並且該節點沒法往更小的區域分配了，此時演算法從扎堆的角點中選出角點響應值最大的關鍵點作為該根結點的關鍵點，經過處理之後形成了右圖所示。
程式碼注釋與理解
該四叉樹的實現在下列函數中實現
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)
vToDistributeKeys：變數中存儲的是從金字塔中某一層影像上提取的特徵點
minX, maxX, minY, maxY：是該層影像去除了邊界的區域
N: mnFeaturesPerLevel[i]：表示該層影像上應該提取的特徵點的個數
level: 該影像處在金字塔上的層數
四叉樹實現ORB管理的區域劃分，也就是實現上述步驟（3）的理論程式碼
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();
}
接下來將循環遍歷已經生成的所有節點，並不斷的判斷根節點是否可再分，也就是實現上述的步驟（4）
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);
else
lit++;
}
最終遍歷所有節點，找到該節點中響應值最大的特徵點進行保存，經過這樣一頓操作，就將影像金字塔中的某一層影像上的角點優化完畢。
