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最大的作为这个结点的代表保留下来,我想的这样一来,万一留下的特征点的数目没有达标怎么办呢? 欲知后事如何,请听下次分晓。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/252511.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

SharePoint Search之(七)Search result- 结果源

在使用搜索引擎的时候。非常多情况下&#xff0c;用户希望限定一下搜索范围&#xff0c;以便更加easy找到想要的结果。在SharePoint 2013的search里&#xff0c;也支持类似的功能&#xff0c;SharePoint 默认提供了几种范围&#xff1a; 在SharePoint&#xff0c;这个叫Search …

旷视砸20亿进军AIoT,发布国内首个机器人协作大脑河图

1 月 16 日&#xff0c;人工智能独角兽旷视科技发布了机器人战略&#xff0c;以及自 2018 年 4 月收购艾瑞思机器人&#xff0c;进军机器人领域的最新进展——智能协同大脑河图。在会上&#xff0c;旷视还大笔一挥&#xff0c;决定投入 20 亿元&#xff0c;用于打造物流仓储上下…

「JupyterLab」 Jupyter Notebook 新生代IDE模式页面

参考&#xff1a;Overview 安装&#xff1a; $ pip install jupyterlab 启动&#xff08;不是jupyter notebook&#xff09;&#xff1a; $ jupyter lab Jupyterlab中最好用的就是显示csv数据。CSV数据显示效果&#xff1a; 安装插件 jupyterlab是和jupyter notebook隔离的&…

PAT(乙级)1009

1009. 说反话 (20)给定一句英语&#xff0c;要求你编写程序&#xff0c;将句中所有单词的顺序颠倒输出。 输入格式&#xff1a;测试输入包含一个测试用例&#xff0c;在一行内给出总长度不超过80的字符串。字符串由若干单词和若干空格组成&#xff0c;其中单词是由英文字母&…

库存扣减问题

2019独角兽企业重金招聘Python工程师标准>>> 并发减库存 并发扣库存问题总结 库存扣减还有这么多方案&#xff1f; | 架构师之路 转载于:https://my.oschina.net/u/2939155/blog/3004363

HSRPSTPACL

1 HSRP配置 1.1 问题 在企业网络到外部的连接方案中&#xff0c;要求不高的条件下可以是单出口。一旦该出口线路出现问题&#xff0c;整个企业网络就不能连接到外网了。为了使得企业网络到外网连接的高可用性&#xff0c;可以设置两个以上的出口&#xff0c;然而多个出口对…

java 的 CopyOnWriteArrayList类

初识CopyOnWriteArrayList 第一次见到CopyOnWriteArrayList&#xff0c;是在研究JDBC的时候&#xff0c;每一个数据库的Driver都是维护在一个CopyOnWriteArrayList中的&#xff0c;为了证明这一点&#xff0c;贴两段代码&#xff0c;第一段在com.mysql.jdbc.Driver下&#xff0…

科技的趋势!AI将进军了37%的企业

2019独角兽企业重金招聘Python工程师标准>>> 市场研究机构Gartner调查了全球89个国家的逾3,000名信息长&#xff08;CIO&#xff09;&#xff0c;显示有37%的企业已经或打算于近期内部署人工智能&#xff08;AI&#xff09;&#xff0c;在4年内成长270%。Gartner研究…

CMakeLists.txt编写规则

在PROJECT_SOURCE_DIR下新建了src, include, lib, bin四个子文件夹。 src文件夹用来存放所有的.cpp文件&#xff0c;include文件夹用来存储所有的.h文件&#xff0c; lib中存放生成的自己编写的共享库&#xff0c; bin中存放所有的可执行文件 用SET来设置.exe可执行文件和共享…

瓜子二手车发12月二手车价格:汉兰达奥德赛CR-V保值率居首

中新网1月22日电 日前&#xff0c;基于海量个人对个人的二手车成交数据&#xff0c;瓜子二手车公布了12月全国及多个核心城市的二手车交易“瓜子价”数据。数据显示&#xff0c;2018年12月全国瓜子二手车严选直卖签约均价为87934元&#xff0c;环比上涨0.16%&#xff0c;同比上…

概率分布之间的距离度量以及python实现(三)

概率分布之间的距离&#xff0c;顾名思义&#xff0c;度量两组样本分布之间的距离 。 1、卡方检验 统计学上的χ2统计量&#xff0c;由于它最初是由英国统计学家Karl Pearson在1900年首次提出的&#xff0c;因此也称之为Pearson χ2&#xff0c;其计算公式为 (i1&#xff0c;2&…

C++求职题

文章大部分内容转载https://www.cnblogs.com/lanxuezaipiao/p/4127904.html 1.冒泡排序法&#xff1a; 如果有N个数字需要排序&#xff0c;那么需要进行(N-1)趟循环&#xff0c;第i趟循环需要对比的次数为(N-i)。所以可以用双重循环&#xff0c;外层循环用于控制循环的趟数&a…

封装一个ViewPager真正的实现图片无限循环滚动带导航点

效果图&#xff1a; 大家在写项目的过程中常常会碰到须要实现Viewpager里面载入几张图片来循环自己主动轮播的效果&#xff0c;假设不封装一下的话代码分散在activity里面会显得非常乱。并且也不利于我们下次复用&#xff0c;所以这里我把viewpager的相关代码抽取出来放在了一个…

毕业论文页眉页脚页码插入

用word这么多年&#xff0c;第一次完整的操作了一遍页眉页脚页码的插入过程&#xff0c;其实三者都要要求奇偶页不同 1.页面布局-》右下角箭头-》版式-》奇偶页不同 因为文章不同的部分需要插入不同的页眉页脚页码&#xff0c;所以要在不同的部分插入分解符断开它们的连接 2、…

巴黎市中心降下2019年第一场雪

当地时间1月22日&#xff0c;法国巴黎市中心降下2019年第一场雪&#xff0c;气温也随之下降&#xff0c;街上的行人和车辆均有所减少。中新社记者 李洋 摄一对情侣在埃菲尔铁塔前合影留念。无家可归者在长椅上睡觉。游客在卢浮宫前拍照。

Echarts实现隐藏x轴,y轴,刻度线,网格

"yAxis": [{//就是一月份这个显示为一个线段&#xff0c;而不是数轴那种一个点点"show" : true,"boundaryGap": true,"type": "category","name": "时间","data": ["1月", "2…

Atom插件主题推荐

注意事项 主题和插件这方面,比 Sublime Text 人性化多了..一些比较用心的作者增加了二度设置功能。 何为二度设置,就是不用手写代码修改配置文件&#xff0c;点点鼠标&#xff0c;填填输入框就能生效&#xff0c;主题以 isotope-ui 这个做例子介绍,看图&#xff1a; 进入二度设…

印尼发生洪灾和山体滑坡 致多人死亡数千人撤离

当地时间1月23日&#xff0c;印尼南苏拉威西省望加锡居民受洪水影像&#xff0c;用竹筏运送摩托车。近日&#xff0c;印尼南苏拉威西省暴雨连连&#xff0c;造成洪灾和山体滑坡。目前&#xff0c;暴雨引发的洪灾和山体滑坡至少已造成8人死亡&#xff0c;数千人被迫撤离家园。。…

pycharm中无法import已经安装的ros中的库

使用pycharm写python程序&#xff0c;无法import 已经安装的ros包&#xff0c;并提示ImportError: No module named sensor_msgs.msg 解决方法如下 1. pycharm->file->settings->project:csvfile->project interpreter-> 点击右侧下三角选择 show all 2. 在弹…