四叉树在SLAM中的应用
本文以ORB-SLAM为例，例子均来自于ORB-SLAM
什么是四叉树？
四叉树或四元树也被称为Q树（Q-Tree）。四叉树广泛应用于图像处理、空间数据索引、2D中的快速碰撞检测、存储稀疏数据等，而八叉树（Octree）主要应用于3D图形处理。对游戏编程，这会很有用。本文着重于对四叉树与八叉树的原理与结构的介绍，帮助您在脑海中建立四叉树与八叉树的基本思想。本文并不对这两种数据结构同时进行详解，而只对四叉树进行详解，因为八叉树的建立可由四叉树的建立推得。
ORB-SLAM中使用四叉树来快速筛选特征点，筛选的目的是非极大值抑制，取局部特征点邻域中FAST角点相应值最大的点，而如何搜索到这些扎堆的特征点，则采用的是四叉树的分快思想，递归找到成群的点，并从中找到相应值最大的点。
图解表示
代码怎么实现的？
在ORBextractor.cc 函数 ORBextractor::DistributeOctTree
第一部分：
1 输入图像 未分的关键点 对应ORBextractor::DistributeOctTree函数中的形参 vToDistributeKeysORBextractor.cc#L537
2 根据图像区域构造初始的 根结点，每个根结点包含图像的一个区域，每个根结点同样包括4个子结点
3 定义一个提取器 ExtractorNode ni;ORBextractor.cc#L552
4 设置提取器节点的图像边界 ni.UL ni.UR ni.BL ni.BRORBextractor.cc#L552-L556
5 将刚才生成的提取节点添加到列表中lNodes.push_back(ni);ORBextractor.cc#L559
6 存储这个初始的提取器节点句柄vpIniNodes[i] = &lNodes.back;ORBextractor.cc#L560
7 将未分的所有关键点分配给 2中构造的 根结点，这样每个根节点都包含了其所负责区域内的 所有关键点按特征点的横轴位置，分配给属于那个图像区域的提取器节点 vpIniNodes[kp.pt.x/hX]->vKeys.push_back(vToDistributeKeys[i]);ORBextractor.cc#L567
8 根结点构成一个 根结点list，代码中是lNodes用来更新与存储所有的根结点 遍历lNodes，标记不可再分的节点，用的标记变量是 lit->bNoMoreORBextractor.cc#L576
第二部分
1 当列表中还有可分的结点区域的时候：while(!bFinish)ORBextractor.cc#L592
2 开始遍历列表中所有的提取器节点，并进行分解或者保留：while(lit!=lNodes.end)ORBextractor.cc#L604
3 判断当前根结点是否可分，可分的意思是，它包含的关键点能够继续分配到其所属的四个子结点所在区域中（左上，右上，左下，右下），代码中是判断标志位if(lit->bNoMore)ORBextractor.cc#L606意思是如果当前的提取器节点具有超过一个的特征点，那么就要进行继续细分
4 如果可分，将分出来的子结点作为新的根结点放入INodes的前部，e.g. lNodes.front.lit = lNodes.begin;ORBextractor.cc#L626，就是在四个if(n*.vKeys.size>0)条件中执行。然后将原先的根结点从列表中删除，e.g.lit=lNodes.erase(lit);ORBextractor.cc#L660。由于新加入的结点是从列表头加入的，不会影响这次的循环，该次循环只会处理当前级别的根结点。
5 当所有结点不可分，e.g(int)lNodes.size==prevSizeORBextractor.cc#L667，或者结点已经超过需要的点(int)lNodes.size>=NORBextractor.cc#L667时，跳出循环bFinish = true;ORBextractor.cc#L669。
参考：https://www.sohu.com/a/439970012_100007727
第三部分
当所有根结点都不可分时，不可分的意思是，该结点包含的关键点，没法再往更小的区域上分配了，那么将不可分的根节点中包含关键点大于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;
}
参考：https://blog.csdn.net/sinat_23093485/article/details/89569982