文章目录
一、函数的作用
ORB-SLAM2 ---- ExtractorNode::DivideNode是此函数的基础，此函数的作用是使用四叉树法对一个图像金字塔图层中的特征点进行平均和分发，这个函数代码量相对较大，但是他在特征提取中是一个非常重要的函数，读者一定要认真仔细的学习。
二、源码及注释
/**
* @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容器
*/
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));
// 一个初始化节点的x方向的像素点个数
const float hX = static_cast<float>(maxX-minX)/nIni;
// 节点的列表
list<ExtractorNode> lNodes;
// 创建节点指针的容器
vector<ExtractorNode*> vpIniNodes;
// 重新设置节点指针容器大小
vpIniNodes.resize(nIni);
// 生成初始提取器
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();
}
//Associate points to childs
// 将特征点分配到提取器节点中
for(size_t i=0;i<vToDistributeKeys.size();i++)
{
// 获取这个特征点对象
const cv::KeyPoint &kp = vToDistributeKeys[i];
// 按特征点横轴的位置，将特征点分配到相应的提取器节点
vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp); // x/hX算出来的数值抛弃小数点后面的值
}
// 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点
list<ExtractorNode>::iterator lit = lNodes.begin();
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用于存储节点的vSize和句柄对
//这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode; // 特征点数目为int类型，句柄为ExtractorNode*类型
// 调整大小，这里的意思是一个初始化节点将“分裂”成为四个
vSizeAndPointerToNode.reserve(lNodes.size()*4);
// 利用四叉树方法对图像进行划分区域，均匀分配特征点
while(!bFinish)
{
// 更新迭代次数，并未起到作用
iteration++;
// 保留当前节点个数
int prevSize = lNodes.size();
// 重新定位迭代器指向的列表头部
lit = lNodes.begin();
// 需要展开的节点计数，这个一直保持累计，不清零
int nToExpand = 0;
//因为是在循环中，前面的循环体中可能污染了这个变量，所以清空
//这个变量也只是统计了某一个循环中的点
//这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
vSizeAndPointerToNode.clear();
// 将目前的子区域进行划分
// 开始遍历列表中所有的提取器节点，并进行分解或者保留
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
// 如果节点中特征点数目不为1，则四分该节点
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);
// 再判断其中子提取器节点中的特征点数目是否大于1，如果有超过一个的特征点，那么待展开的节点计数加1
if(n1.vKeys.size()>1)
{
nToExpand++;
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)
{
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、当前所有的节点中都只包含一个特征点
// prevSize中保存的是分裂之前的节点个数，如果分裂之前和分裂之后的总节点个数一样，说明当前所有的节点区域中只有一个特征点，已经不能够再细分了
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
{
bFinish = true;
}
// 如果分裂后总共的节点数大于N，则进入循环，这里N是传的参
// ？那不大于N会怎么样
else if(((int)lNodes.size()+nToExpand*3)>N)
{
while(!bFinish)
{
// 记录当前节点个数
prevSize = lNodes.size();
// 记录这些还可以分裂的节点信息
vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode; // pair对里面有两个元素，第一个为int类型，第二个为ExtractorNode*类型
// 将容器清空
vSizeAndPointerToNode.clear();
//! 注意这里的排序规则非常重要！会导致每次最后产生的特征点都不一样。建议使用 stable_sort
sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
// 遍历这个存储了pair对的vector，注意是从后往前遍历
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
// 如果节点内的特征点数量大于0，则进行下一步
if(n1.vKeys.size()>0)
{
// 将该节点放入节点列表中
lNodes.push_front(n1);
// 如果节点内特征点数大于1，则就将这个节点放入vSizeAndPointerToNode中，以便再次循环分裂
if(n1.vKeys.size()>1)
{
vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front())); //make_pair与pair用法一样，区别是make_pair可以自动识别类型
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来存储我们感兴趣的特征点的过滤结果
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个关键点响应值初始化最大响应值
float maxResponse = pKP->response;
for(size_t k=1;k<vNodeKeys.size();k++)
{
// 如果该点的回应值大于最大的回应值，则将pKP指向该值，并将其回应值重置为最大回应值以供后续比较
if(vNodeKeys[k].response>maxResponse)
{
pKP = &vNodeKeys[k];
maxResponse = vNodeKeys[k].response;
}
}
// 将该节点内响应值最大的特征点放入最终容器
vResultKeys.push_back(*pKP);
}
// 返回这个最终的特征点容器
return vResultKeys;
}
三、函数的讲解
因为此函数代码量较大，我们分几段来讲解
1. 根据宽高比确定初始节点数目
这段代码是根据高宽比确定初始节点数目（一般是30*30的网格），这个数目用四舍五入的方法（如下图），但是此处有一个bug，当maxX小于maxY的时候，nIni = 0，这个会报错。
const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));
// 一个初始化节点的x方向的像素点个数
const float hX = static_cast<float>(maxX-minX)/nIni;
// 节点的列表
list<ExtractorNode> lNodes;
// 创建节点指针的容器
vector<ExtractorNode*> vpIniNodes;
// 重新设置节点指针容器大小
vpIniNodes.resize(nIni);
2. 生成初始提取器并将特征点分配到提取器中
第一个for循环是生成提取器，坐标的分配和ExtractorNode::DivideNode相同，不再赘述，第二个循环是将特征点分配到提取器中。
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();
}
for(size_t i=0;i<vToDistributeKeys.size();i++)
{
// 获取这个特征点对象
const cv::KeyPoint &kp = vToDistributeKeys[i];
// 按特征点横轴的位置，将特征点分配到相应的提取器节点
vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp); // x/hX算出来的数值抛弃小数点后面的值
}
3. 遍历此提取器节点列表，标记那些不可再分裂的节点，删除那些没有分配到特征点的节点
list<ExtractorNode>::iterator lit = lNodes.begin();
while(lit!=lNodes.end())
{
// 如国特征点数等于1，则不可再分，lit->bNoMore=true（标记）
if(lit->vKeys.size()==1)
{
lit->bNoMore=true;
lit++;
}
如果特征点数为0，则删除这个节点
else if(lit->vKeys.empty())
lit = lNodes.erase(lit);
else
lit++;
}
// 结束标志位清空
bool bFinish = false;
// 记录迭代次数，并未起到作用
int iteration = 0;
//声明一个vector用于存储节点的vSize和句柄对
//这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode; // 特征点数目为int类型，句柄为ExtractorNode*类型
// 调整大小，这里的意思是一个初始化节点将“分裂”成为四个
vSizeAndPointerToNode.reserve(lNodes.size()*4);
4. 利用四叉树方法对图像进行划分区域，均匀分配特征点
这个部分代码看起来很多，但其实很多都是在重复，其讲述的是，将不空的节点不停的分裂，子结点中无特征点就删除该节点，有一个就不可再分，大于一个就继续4分，要想这个循环结束，需要满足两个条件中的一个：
- 节点数量大于或等于需要的特征点数量
- 每个保留下来的节点中都只有一个特征点
while(!bFinish)
{
// 更新迭代次数，并未起到作用
iteration++;
// 保留当前节点个数
int prevSize = lNodes.size();
// 重新定位迭代器指向的列表头部
lit = lNodes.begin();
// 需要展开的节点计数，这个一直保持累计，不清零
int nToExpand = 0;
//因为是在循环中，前面的循环体中可能污染了这个变量，所以清空
//这个变量也只是统计了某一个循环中的点
//这个变量记录了在一次分裂循环中，那些可以再继续进行分裂的节点中包含的特征点数目和其句柄
vSizeAndPointerToNode.clear();
// 将目前的子区域进行划分
// 开始遍历列表中所有的提取器节点，并进行分解或者保留
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
// 如果节点中特征点数目不为1，则四分该节点
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);
// 再判断其中子提取器节点中的特征点数目是否大于1，如果有超过一个的特征点，那么待展开的节点计数加1
if(n1.vKeys.size()>1)
{
nToExpand++;
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)
{
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、当前所有的节点中都只包含一个特征点
// prevSize中保存的是分裂之前的节点个数，如果分裂之前和分裂之后的总节点个数一样，说明当前所有的节点区域中只有一个特征点，已经不能够再细分了
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
{
bFinish = true;
}
// 如果分裂后总共的节点数大于N，则进入循环，这里N是传的参
// ？那不大于N会怎么样
else if(((int)lNodes.size()+nToExpand*3)>N)
{
while(!bFinish)
{
// 记录当前节点个数
prevSize = lNodes.size();
// 记录这些还可以分裂的节点信息
vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode; // pair对里面有两个元素，第一个为int类型，第二个为ExtractorNode*类型
// 将容器清空
vSizeAndPointerToNode.clear();
//! 注意这里的排序规则非常重要！会导致每次最后产生的特征点都不一样。建议使用 stable_sort
sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
// 遍历这个存储了pair对的vector，注意是从后往前遍历
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
// 如果节点内的特征点数量大于0，则进行下一步
if(n1.vKeys.size()>0)
{
// 将该节点放入节点列表中
lNodes.push_front(n1);
// 如果节点内特征点数大于1，则就将这个节点放入vSizeAndPointerToNode中，以便再次循环分裂
if(n1.vKeys.size()>1)
{
vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front())); //make_pair与pair用法一样，区别是make_pair可以自动识别类型
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;
}
}
}
5.从每个节点中选出质量最好的一个点
从上图我们知道，分完节点后可能一个节点中有多个特征点，这样会导致特征点过于密集，会影响建图的精度，接下来这段代码的含义是，每个节点内保留一个应答最大的特征带点，并将其装入容器内，作为函数的返回值。
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个关键点响应值初始化最大响应值
float maxResponse = pKP->response;
for(size_t k=1;k<vNodeKeys.size();k++)
{
// 如果该点的回应值大于最大的回应值，则将pKP指向该值，并将其回应值重置为最大回应值以供后续比较
if(vNodeKeys[k].response>maxResponse)
{
pKP = &vNodeKeys[k];
maxResponse = vNodeKeys[k].response;
}
}
// 将该节点内响应值最大的特征点放入最终容器
vResultKeys.push_back(*pKP);
}
// 返回这个最终的特征点容器
return vResultKeys;
扫一扫
1971
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
