ORB-SLAM中四叉树管理角点
四叉树索引的基本思想是将地理空间递归划分为不同层次的树结构。它将已知范围的空间等分成四个相等的子空间,如此递归下去,直至树的层次达到一定深度或者满足某种要求后停止分割。
四叉树对于区域查询,效率比较高。但如果空间对象分布不均匀,随着地理空间对象的不断插入,四叉树的层次会不断地加深,将形成一棵严重不平衡的四叉树,那么每次查询的深度将大大的增多,从而导致查询效率的急剧下降。
四叉树的定义
四元树又称四叉树是一种树状数据结构,在每一个节点上会有四个子区块。四元树常应用于二维空间数据的分析与分类。它将数据区分成为四个象限。数据范围可以是方形或矩形或其他任意形状。
这种数据结构是由 拉斐尔·芬科尔(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++;
}
最终遍历所有节点,找到该节点中响应值最大的特征点进行保存,经过这样一顿操作,就将图像金字塔中的某一层图像上的角点优化完毕。
历史相关文章
以上内容是在阅读大量网络优秀博客与资料并结合ORB-SLAM的代码的个人理解,如有错误,欢迎指出,更期待有人能够指出理解不当之处,能够一起交流学习。并且欢迎更多的SLAM爱好者能够加入我们交流学习小组,多人负责不同的模块,定期分享与交流。具体加入方式请查看第三期:一起来学SLAM
平台活动指南
公众号将会推送基于PCL库的点云处理,SLAM,三维视觉,高精地图相关的文章。公众号致力于理解三维世界相关内容的干货分享。不仅组织技术交流群,并且组建github组群,可自由分享。交流提问。
历史文章查看点云学习历史文章大汇总
1.一起学SLAM:第三期:一起来学SLAM
2.招募乐于分享的你:点云PCL运营招募啦
3.招募计算机视觉,SLAM,三维视觉,点云等相关领域博客博主,或者公司开设专栏,只要与平台主题相关,乐于分享,都可以与本平台合作经营,发布原创文章。并且可以加入微信,QQ交流群,认识更多志同道合的一起同行分享。
4,相机测评活动:图漾双目,小觅相机,奥比中光三款相机测评活动正在进行中,,,,,,