本文系ORB-SLAM2原理+代码实战系列原创文章，对应的视频课程见：
本文系ORB-SLAM2原理+代码实战系列原创文章，对应的视频课程见：
大家好，从今天开始我们陆续更新ORB-SLAM2/3系列的原创文章，以小白和师兄对话的形式阐述 背景原理+代码解析， 喜欢的点个赞分享，支持的人越多，更新越有动力！如有错误欢迎留言指正！
接上回继续。。。
四叉树实现特征点均匀化分布
师兄：四叉树实现特征均匀化分布的方法是重点，也是一个难点，我先讲一下步骤和原理：
第1步：首先确定初始的节点（node）数目。根据图像宽高比取整来确定，所以一般的VGA ( ) 分辨率图像刚开始的时候只有一个节点，也是四叉树的根节点。
下面我们用一个具体的例子来分析四叉树是如何帮助我们均匀化选取特定数目的特征点的。假设初始节点只有1个，那么所有的特征点都属于该节点。我们目标是均匀的选取 25 个特征点，那么后面我们就需要分裂出25个节点，然后从每个节点中选取一个代表性的特征点。
第2步：节点第1次分裂，1个根节点分裂为4个节点。如下图所示，分裂之后根据图像的尺寸划分节点的区域，对应的边界为 ，分别对应左上角、右上角、左下角、右下角的四个坐标。有些坐标会被多个节点共享，比如图像中心点坐标就同时被 四个点共享。落在某个节点区域范围内的所有特征点都属于该节点的元素。
然后统计每个节点里包含特征点的数目，如果某个节点里特征点数目为 0，则删掉该节点，如果某个节点里特征点数目为 1，则该节点不再进行分裂。判断此时的节点总数是否超过设定值 25，如果没有超过则继续对每个节点分裂。
这里需要注意的是一个母节点分裂为 4 个子节点后，需要在节点链表里删掉原来的母节点，所以实际上一次分裂净增加了 3 个节点。所以下次分裂后节点的总数我们是可以提前预估的，计算方式为：（当前节点总数 + 即将分裂的节点总数 3 ），对于图示来说，下次分裂最多可以得到 个节点，显然还是没有达到 25 的要求，需要继续分裂。
第3步：对上一步得到的 4 个节点分别进行一分为四的操作，然后统计分裂后的每个节点里包含特征点的数目，我们可以看到已经有 2 个节点里的特征点数目为 0，于是在节点链表里删掉这 2 个节点（下图中标记为 ）。如果某个节点里特征点数目为 1，则该节点不再进行分裂。此次分裂总共得到 14 个节点。
第4步：上一步得到的 14 个节点继续进行一分四的操作。预计这次分裂最多可以得到 个节点，已经超过我们需要提取 25 个特征点数目的需求。此时需要注意了，我们不需要把所有的节点都进行分裂，我们只需要在分裂得到的所有节点数目刚刚达到 25 时，即可停止分裂，这样操作的目的一方面是可以避免多分裂后再删除而做无用功，另一方面，因为是指数级分裂，所以也大大加速了四叉树分裂的过程。
那么，如何选取分裂的顺序呢？源码里采用的策略是对所有节点按照内部包含的特征点数目进行排列，优先分裂特征点数目多的节点，这样做的目的是使得特征密集的区域能够更加细分。对于包含特征点较少的节点，有可能因为提前达到要求而不再分裂。下图中绿色方框内的节点就是因为包含的特征点数目太少（这里包括只有 1 个也不再分裂的情况），分裂的优先级很低，最终在达到要求的节点数目前没有再分裂。
第5步：上一步中我们已经得到了所需要的 25 个节点，只需要从每个节点中选出角点响应值最高的特征点，作为该节点的唯一特征点，该节点内其他低响应值的特征点全部删掉。这样我们就得到了均匀化后的、需要数目的特征点。
以上就是使用四叉树对图像特征点进行均匀化的原理，详细注释代码见：
/**
* @brief 使用四叉树法对一个图像金字塔图层中的特征点进行平均和分发
*
* @param[in] vToDistributeKeys 等待进行分配到四叉树中的特征点
* @param[in] minX 当前图层的图像的边界
* @param[in] maxX
* @param[in] minY
* @param[in] maxY
* @param[in] N 希望提取出的特征点个数
* @param[in] level 指定的金字塔图层
* @return vector<cv::KeyPoint> 已经均匀分散好的特征点容器
*/
vector<cv::KeyPoint> ORBextractor::DistributeOctTree( constvector<cv::KeyPoint>& vToDistributeKeys, constint&minX,
constint&maxX, constint&minY, constint&maxY, constint&N, constint&level)
{
// Step 1 根据宽高比确定初始节点数目
//计算应该生成的初始节点个数，根节点的数量nIni是根据边界的宽高比值确定的，一般是1或者2
// ! bug: 如果宽高比小于0.5，nIni=0, 后面hx会报错
constintnIni = round( static_cast< float>(maxX-minX)/(maxY-minY));
//一个初始的节点的x方向有多少个像素
constfloathX = static_cast< float>(maxX-minX)/nIni;
//存储有提取器节点的链表
list<ExtractorNode> lNodes;
//存储初始提取器节点指针的vector
vector<ExtractorNode*> vpIniNodes;
//重新设置其大小
vpIniNodes.resize(nIni);
// Step 2 生成初始提取器节点
for( inti= 0; i<nIni; i++)
{
//生成一个提取器节点
ExtractorNode ni;
//设置提取器节点的图像边界
ni.UL = cv::Point2i(hX* static_cast< float>(i), 0); //UpLeft
ni.UR = cv::Point2i(hX* static_cast< float>(i+ 1), 0); //UpRight
ni.BL = cv::Point2i(ni.UL.x,maxY-minY); //BottomLeft
ni.BR = cv::Point2i(ni.UR.x,maxY-minY); //BottomRight
//重设vkeys大小
ni.vKeys.reserve(vToDistributeKeys.size);
//将刚才生成的提取节点添加到链表中
lNodes.push_back(ni);
//存储这个初始的提取器节点句柄
vpIniNodes[i] = &lNodes.back;
}
// Step 3 将特征点分配到子提取器节点中
for( size_ti= 0;i<vToDistributeKeys.size;i++)
{
//获取这个特征点对象
constcv::KeyPoint &kp = vToDistributeKeys[i];
//按特征点的横轴位置，分配给属于那个图像区域的提取器节点（最初的提取器节点）
vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
}
// Step 4 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点
list<ExtractorNode>::iterator lit = lNodes.begin;
while(lit!=lNodes.end)
{
//如果初始的提取器节点所分配到的特征点个数为1
if(lit->vKeys.size== 1)
{
//那么就标志位置位，表示此节点不可再分
lit->bNoMore= true;
//更新迭代器
lit++;
}
//如果一个提取器节点没有被分配到特征点，那么就从列表中直接删除它
elseif(lit->vKeys.empty)
//注意，由于是直接删除了它，所以这里的迭代器没有必要更新；否则反而会造成跳过元素的情况
lit = lNodes.erase(lit);
else
//如果上面的这些情况和当前的特征点提取器节点无关，那么就只是更新迭代器
lit++;
}
//结束标志位清空
boolbFinish = false;
//记录迭代次数，只是记录，并未起到作用
intiteration = 0;
//声明一个vector用于存储节点的vSize和句柄对
//这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
vector<pair< int,ExtractorNode*> > vSizeAndPointerToNode;
//调整大小，这里的意思是一个初始化节点将“分裂”成为四个
vSizeAndPointerToNode.reserve(lNodes.size* 4);
// Step 5 利用四叉树方法对图像进行划分区域，均匀分配特征点
while(!bFinish)
{
//更新迭代次数计数器，只是记录，并未起到作用
iteration++;
//保存当前节点个数，prev在这里理解为“保留”比较好
intprevSize = lNodes.size;
//重新定位迭代器指向列表头部
lit = lNodes.begin;
//需要展开的节点计数，这个一直保持累计，不清零
intnToExpand = 0;
//因为是在循环中，前面的循环体中可能污染了这个变量，所以清空
//这个变量也只是统计了某一个循环中的点
//这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
vSizeAndPointerToNode.clear;
//将目前的子区域进行划分
//开始遍历列表中所有的提取器节点，并进行分解或者保留
while(lit!=lNodes.end)
{
//如果提取器节点只有一个特征点，
if(lit->bNoMore)
{
//那么就没有必要再进行细分了
lit++;
//跳过当前节点，继续下一个
continue;
}
else
{
//如果当前的提取器节点具有超过一个的特征点，那么就要进行继续分裂
ExtractorNode n1,n2,n3,n4;
//再细分成四个子区域
lit->DivideNode(n1,n2,n3,n4);
//如果这里分出来的子区域中有特征点，那么就将这个子区域的节点添加到提取器节点的列表中
//注意这里的条件是，有特征点即可
if(n1.vKeys.size> 0)
{
//注意这里也是添加到列表前面的
lNodes.push_front(n1);
//再判断其中子提取器节点中的特征点数目是否大于1
if(n1.vKeys.size> 1)
{
//如果有超过一个的特征点，那么待展开的节点计数加1
nToExpand++;
//保存这个特征点数目和节点指针的信息
vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size,&lNodes.front));
// lNodes.front.lit 和前面的迭代的lit 不同，只是名字相同而已
// lNodes.front.lit是node结构体里的一个指针用来记录节点的位置
// 迭代的lit 是while循环里作者命名的遍历的指针名称
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;
}
}
//当这个母节点expand之后就从列表中删除它了，能够进行分裂操作说明至少有一个子节点的区域中特征点的数量是>1的
// 分裂方式是后加的节点先分裂，先加的后分裂
lit=lNodes.erase(lit);
//继续下一次循环，其实这里加不加这句话的作用都是一样的
continue;
} //判断当前遍历到的节点中是否有超过一个的特征点
} //遍历列表中的所有提取器节点
//停止这个过程的条件有两个，满足其中一个即可：
//1、当前的节点数已经超过了要求的特征点数
//2、当前所有的节点中都只包含一个特征点
if(( int)lNodes.size>=N //判断是否超过了要求的特征点数
|| ( int)lNodes.size==prevSize) //prevSize中保存的是分裂之前的节点个数，如果分裂之前和分裂之后的总节点个数一样，说明当前所有的
//节点区域中只有一个特征点，已经不能够再细分了
{
//停止标志置位
bFinish = true;
}
// Step 6 当再划分之后所有的Node数大于要求数目时,就慢慢划分直到使其刚刚达到或者超过要求的特征点个数
//可以展开的子节点个数nToExpand x3，是因为一分四之后，会删除原来的主节点，所以乘以3
elseif((( int)lNodes.size+nToExpand* 3)>N)
{
//如果再分裂一次那么数目就要超了，这里想办法尽可能使其刚刚达到或者超过要求的特征点个数时就退出
//这里的nToExpand和vSizeAndPointerToNode不是一次循环对一次循环的关系，而是前者是累计计数，后者只保存某一个循环的
//一直循环，直到结束标志位被置位
while(!bFinish)
{
//获取当前的list中的节点个数
prevSize = lNodes.size;
//保留那些还可以分裂的节点的信息, 这里是深拷贝
vector<pair< int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
//清空
vSizeAndPointerToNode.clear;
// 对需要划分的节点进行排序，对pair对的第一个元素进行排序，默认是从小到大排序
// 优先分裂特征点多的节点，使得特征点密集的区域保留更少的特征点
//! 注意这里的排序规则非常重要！会导致每次最后产生的特征点都不一样。建议使用 stable_sort
sort(vPrevSizeAndPointerToNode.begin,vPrevSizeAndPointerToNode.end);
//遍历这个存储了pair对的vector，注意是从后往前遍历
for( intj=vPrevSizeAndPointerToNode.size -1;j>= 0;j--)
{
ExtractorNode n1,n2,n3,n4;
//对每个需要进行分裂的节点进行分裂
vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4);
//其实这里的节点可以说是二级子节点了，执行和前面一样的操作
if(n1.vKeys.size> 0)
{
lNodes.push_front(n1);
if(n1.vKeys.size> 1)
{
//因为这里还有对于vSizeAndPointerToNode的操作，所以前面才会备份vSizeAndPointerToNode中的数据
//为可能的、后续的又一次for循环做准备
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;
} //遍历vPrevSizeAndPointerToNode并对其中指定的node进行分裂，直到刚刚达到或者超过要求的特征点个数
//这里理想中应该是一个for循环就能够达成结束条件了，但是作者想的可能是，有些子节点所在的区域会没有特征点，因此很有可能一次for循环之后
//的数目还是不能够满足要求，所以还是需要判断结束条件并且再来一次
//判断是否达到了停止条件
if(( int)lNodes.size>=N || ( int)lNodes.size==prevSize)
bFinish = true;
} //一直进行nToExpand累加的节点分裂过程，直到分裂后的nodes数目刚刚达到或者超过要求的特征点数目
} //当本次分裂后达不到结束条件但是再进行一次完整的分裂之后就可以达到结束条件时
} // 根据兴趣点分布,利用4叉树方法对图像进行划分区域
// Step 7 保留每个区域响应值最大的一个兴趣点
//使用这个vector来存储我们感兴趣的特征点的过滤结果
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];
//用第1个关键点响应值初始化最大响应值
floatmaxResponse = pKP->response;
//开始遍历这个节点区域中的特征点容器中的特征点，注意是从1开始哟，0已经用过了
for( size_tk= 1;k<vNodeKeys.size;k++)
{
//更新最大响应值
if(vNodeKeys[k].response>maxResponse)
{
//更新pKP指向具有最大响应值的keypoints
pKP = &vNodeKeys[k];
maxResponse = vNodeKeys[k].response;
}
}
//将这个节点区域中的响应值最大的特征点加入最终结果容器
vResultKeys.push_back(*pKP);
}
//返回最终结果容器，其中保存有分裂出来的区域中，我们最感兴趣、响应值最大的特征点
returnvResultKeys;
}
欢迎支持独家视频课程： 如何真正搞透视觉SLAM？ 返回搜狐，查看更多