本博客结合哔哩大学视频ORBSLAM2【ORBSLAM2源码讲解专题一】ORB特征点提取与均匀化策略和高翔的《视觉SLAM十四讲》总结。
代码参照github的ORB_SLAM2_detailed_comments
ORBSLAM2代码很经典,而且代码量大,会分成多个博客研究,下为slam专题的其他链接:
代码要求环境做了总结,解决了配置中的一些常见问题:slam的环境配置大全–保姆教学
代码如何运行记录:ORB_SLAM2代码的简介安装运行
特征点法:我们需要从图像中选取比较有代表性的点,这些点在相机发生少量视角变化时会保持不变,在此基础上讨论相机位姿估计问题,在经典的SLAM模型中我们称他们为路标,在视觉SLAM中路标则是图像特征(feature)。
特征点: 特征点是图像里一些特别的地方,我们可以把图像中的角点、边缘和区块都当成图像中的代表性的地方。为此,计算机视觉领域研究出了很多特征点的提取方法,如著名的SIFT、SURF、ORB,这些人工设计的特征点有一下特点:
关键点和描述子: 特征点由关键点(Key-point)和描述子(Descriptor)两部分组成,比如当要谈论SIFT特征时,是指“提取SIFT关键点,并计算SIFT描述子”两件事情。关键点是指该特征点在图像里的位置,有些特征点还具有朝向、大小等信息。描述子通常是一个向量,按照认为的方式描述该点周围像素的信息。描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。
ORB特征: 不同的图像特征特点不同。例如SIFT充分考虑了图像变换中的关照、尺度等变化,很精确但计算量很大,普通的CPU无法实时计算SIFT特征,进行定位建图。ORB适当的降低了精度和健壮性以提升计算速度,在同一图像中提取1000个特征点,ORB要15.3ms,SURF要217.3ms,SIFT约花费5228.7ms。ORB特征也是由关键点和描述子组成,他的关键点称为“Oriented FAST”,Oriented FAST的详细描述在我的另一篇博客Oriented Fast神奇高效的代码实现方式——ORBSLAM2源码讲解(二)。他的描述子称为BRIEF(Binary Robust Independent Elementary Feature)。因此,提取ORB特征分为如下两个步骤:
FAST角点提取:找出图像中的“角点”。相较于原版的FAST,ORB中计算了特征点的主方向,为后续的BRIEF描述子增加了旋转不变的特性。
BRIEF描述子:对前一步提取出特征点的周围图像区域进行描述。
其中ORB特征的描述为本博客的主要内容。
ORB特征提取相比较与其他的特征提取法有什么优势呢?
ORB-SLAM2中的ORB特征均匀化提取方法 vs. OpenCV中的ORB函数:
• 提高了ORB-SLAM2的轨迹精度和鲁棒性
• 两者运行时间差别不大
• 增加特征提取的均匀性可以提高系统精度
• 但是可能会降低特征提取的重复性
有一篇知乎的文章对此做了总结:[ORB-SLAM2] ORB特征提取策略对ORB-SLAM2性能的影响
综上没有直接用opencv里的提取函数,而是采用ORB提取,如何实现这些优质的特性呢?和opencv有什么区别呢?详见下文。
这个图像金字塔的作用是实现尺度的不变性,如下图所示:
蓝色的是图像,每往上走一层就长宽缩小二分之一,左一的红圈是实际中的一个区域在蓝色图中的成像,相机就在虚线位置。若镜头往前推,则这个点就变大了,每一层都跟着变大,但第二列变大后的第二层和第一列的第一层是一样大的,我们做特征匹配时,就可以拿第二列的第二层和第一列的第一层相匹配,这俩差距不大。若拿第一列的第一层和第二列的第一层匹配,因为差距过大是匹配不上的。同样第三列的第一层和第一列的第二层相匹配。由此可以实现无论镜头怎么移动,都可以相匹配。
如下图所示,灰色区域的是原图,FAST特征点提取的时候需要以3pixels的半径的特征点去提取,所以对外扩充一个3的边,形成中间的方框。为了进行高斯模糊,所以又扩充了一个最大的框。
这一扩充功能对应的代码在src包的ORBextractor.cc的1682行。其注释如下:
void ORBextractor::ComputePyramid(cv::Mat image)
{
//开始遍历所有的图层
for (int level = 0; level < nlevels; ++level)
{
//获取本层图像的缩放系数
float scale = mvInvScaleFactor[level];
//计算本层图像的像素尺寸大小
Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
//全尺寸图像。包括无效图像区域的大小。将图像进行“补边”,EDGE_THRESHOLD区域外的图像不进行FAST角点检测
Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);
// 定义了两个变量:temp是扩展了边界的图像,masktemp并未使用
Mat temp(wholeSize, image.type()), masktemp;
// mvImagePyramid 刚开始时是个空的vector
// 把图像金字塔该图层的图像指针mvImagePyramid指向temp的中间部分(这里为浅拷贝,内存相同)
mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
// Compute the resized image
//计算第0层以上resize后的图像
if( level != 0 )
{
//将上一层金字塔图像根据设定sz缩放到当前层级
resize(mvImagePyramid[level-1], //输入图像
mvImagePyramid[level], //输出图像
sz, //输出图像的尺寸
0, //水平方向上的缩放系数,留0表示自动计算
0, //垂直方向上的缩放系数,留0表示自动计算
cv::INTER_LINEAR); //图像缩放的差值算法类型,这里的是线性插值算法
// //! 原代码mvImagePyramid 并未扩充,上面resize应该改为如下
// resize(image, //输入图像
// mvImagePyramid[level], //输出图像
// sz, //输出图像的尺寸
// 0, //水平方向上的缩放系数,留0表示自动计算
// 0, //垂直方向上的缩放系数,留0表示自动计算
// cv::INTER_LINEAR); //图像缩放的差值算法类型,这里的是线性插值算法
//把源图像拷贝到目的图像的中央,四面填充指定的像素。图片如果已经拷贝到中间,只填充边界
//这样做是为了能够正确提取边界的FAST角点
//EDGE_THRESHOLD指的这个边界的宽度,由于这个边界之外的像素不是原图像素而是算法生成出来的,所以不能够在EDGE_THRESHOLD之外提取特征点
copyMakeBorder(mvImagePyramid[level], //源图像
temp, //目标图像(此时其实就已经有大了一圈的尺寸了)
EDGE_THRESHOLD, EDGE_THRESHOLD, //top & bottom 需要扩展的border大小
EDGE_THRESHOLD, EDGE_THRESHOLD, //left & right 需要扩展的border大小
BORDER_REFLECT_101+BORDER_ISOLATED); //扩充方式,opencv给出的解释:
/*Various border types, image boundaries are denoted with '|'
* BORDER_REPLICATE: aaaaaa|abcdefgh|hhhhhhh
* BORDER_REFLECT: fedcba|abcdefgh|hgfedcb
* BORDER_REFLECT_101: gfedcb|abcdefgh|gfedcba
* BORDER_WRAP: cdefgh|abcdefgh|abcdefg
* BORDER_CONSTANT: iiiiii|abcdefgh|iiiiiii with some specified 'i'
*/
//BORDER_ISOLATED 表示对整个图像进行操作
// https://docs.opencv.org/3.4.4/d2/de8/group__core__array.html#ga2ac1049c2c3dd25c2b41bffe17658a36
}
else
{
//对于第0层未缩放图像,直接将图像深拷贝到temp的中间,并且对其周围进行边界扩展。此时temp就是对原图扩展后的图像
copyMakeBorder(image, //这里是原图像
temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
BORDER_REFLECT_101);
}
// //! 原代码mvImagePyramid 并未扩充,应该添加下面一行代码
// mvImagePyramid[level] = temp;
}
}
如何实现特征点的均匀化呢?如下图中要提取25个特征点,如何让这个25个特征点更均匀?
如下长图分五步实现:第一步只有一个node,整个就是一个节点。第二步,我们需要进行分裂,分成四份,总共有四个节点。因为我们想要的到25个特征,每个特征在一个节点内,所以4<25,还要继续分。第三步,继续分,16个节点出现一个节点内没有特征点,无效删除,16<25继续分。第四步继续分,本来有64个,除去无效的,30>25,所以结束分裂。第五步,从每个node里选择一个质量最好的点,比如左上有唯一特征点,不用挑了。左侧第二个,有两个特征点,选择响应值比较高的,最后选出来就是第五步图所示。
此处借助知乎的一篇文章:ORB-SLAM中的ORB特征(提取)
提取流程概览:
原理比较简单,代码实现起来比较复杂,上面方法的代码注释解析如下(在src文件下的ORBextractor.cc的715行开始):
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
// Step 1 根据宽高比确定初始节点数目
//计算应该生成的初始节点个数,根节点的数量nIni是根据边界的宽高比值确定的,一般是1或者2
// ! bug: 如果宽高比小于0.5,nIni=0, 后面hx会报错
const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY)); //此处的maxX等在“ORB提取扩展图像”的图片上,round为取整。
//一个初始的节点的x方向有多少个像素
const float hX = static_cast<float>(maxX-minX)/nIni;
//存储有提取器节点的链表
list<ExtractorNode> lNodes;
//存储初始提取器节点指针的vector
vector<ExtractorNode*> vpIniNodes;
//重新设置其大小
vpIniNodes.resize(nIni);
// Step 2 生成初始提取器节点
for(int i=0; i<nIni; i++)
{
//生成一个提取器节点
ExtractorNode ni;
//设置提取器节点的图像边界
//注意这里和提取FAST角点区域相同,都是“半径扩充图像”,特征点坐标从0 开始
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());
//将刚才生成的提取节点添加到链表中
//虽然这里的ni是局部变量,但是由于这里的push_back()是拷贝参数的内容到一个新的对象中然后再添加到列表中
//所以当本函数退出之后这里的内存不会成为“野指针”
lNodes.push_back(ni);
//存储这个初始的提取器节点句柄
vpIniNodes[i] = &lNodes.back();
}
//Associate points to childs
// Step 3 将特征点分配到子提取器节点中
for(size_t i=0;i<vToDistributeKeys.size();i++)
{
//获取这个特征点对象
const cv::KeyPoint &kp = vToDistributeKeys[i];
//按特征点的横轴位置,分配给属于那个图像区域的提取器节点(最初的提取器节点)
vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
}
// Step 4 遍历此提取器节点列表,标记那些不可再分裂的节点,删除那些没有分配到特征点的节点
// ? 这个步骤是必要的吗?感觉可以省略,通过判断nIni个数和vKeys.size() 就可以吧
list<ExtractorNode>::iterator lit = lNodes.begin();
while(lit!=lNodes.end())
{
//如果初始的提取器节点所分配到的特征点个数为1
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;
//调整大小,这里的意思是一个初始化节点将“分裂”成为四个
vSizeAndPointerToNode.reserve(lNodes.size()*4);
// Step 5 利用四叉树方法对图像进行划分区域,均匀分配特征点
while(!bFinish)
{
//更新迭代次数计数器,只是记录,并未起到作用
iteration++;
//保存当前节点个数,prev在这里理解为“保留”比较好
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
//如果当前的提取器节点具有超过一个的特征点,那么就要进行继续分裂
ExtractorNode n1,n2,n3,n4;
//再细分成四个子区域,DivideNode函数是进行结点划分
lit->DivideNode(n1,n2,n3,n4);
// Add childs if they contain points
//如果这里分出来的子区域中有特征点,那么就将这个子区域的节点添加到提取器节点的列表中
//注意这里的条件是,有特征点即可
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;
}//判断当前遍历到的节点中是否有超过一个的特征点
}//遍历列表中的所有提取器节点
// Finish if there are more nodes than required features or all nodes contain just one point
//停止这个过程的条件有两个,满足其中一个即可:
//1、当前的节点数已经超过了要求的特征点数
//2、当前所有的节点中都只包含一个特征点
if((int)lNodes.size()>=N //判断是否超过了要求的特征点数
|| (int)lNodes.size()==prevSize) //prevSize中保存的是分裂之前的节点个数,如果分裂之前和分裂之后的总节点个数一样,说明当前所有的
//节点区域中只有一个特征点,已经不能够再细分了
{
//停止标志置位
bFinish = true;
}
// Step 6 当再划分之后所有的Node数大于要求数目时,就慢慢划分直到使其刚刚达到或者超过要求的特征点个数
//可以展开的子节点个数nToExpand x3,是因为一分四之后,会删除原来的主节点,所以乘以3
/**
* //?BUG 但是我觉得这里有BUG,虽然最终作者也给误打误撞、稀里糊涂地修复了
* 注意到,这里的nToExpand变量在前面的执行过程中是一直处于累计状态的,如果因为特征点个数太少,跳过了下面的else-if,又进行了一次上面的遍历
* list的操作之后,lNodes.size()增加了,但是nToExpand也增加了,尤其是在很多次操作之后,下面的表达式:
* ((int)lNodes.size()+nToExpand*3)>N
* 会很快就被满足,但是此时只进行一次对vSizeAndPointerToNode中点进行分裂的操作是肯定不够的;
* 理想中,作者下面的for理论上只要执行一次就能满足,不过作者所考虑的“不理想情况”应该是分裂后出现的节点所在区域可能没有特征点,因此将for
* 循环放在了一个while循环里面,通过再次进行for循环、再分裂一次解决这个问题。而我所考虑的“不理想情况”则是因为前面的一次对vSizeAndPointerToNode
* 中的特征点进行for循环不够,需要将其放在另外一个循环(也就是作者所写的while循环)中不断尝试直到达到退出条件。
* */
else if(((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(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的操作,所以前面才会备份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);
//判断是是否超过了需要的特征点数?是的话就退出,不是的话就继续这个分裂过程,直到刚刚达到或者超过要求的特征点个数
//作者的思想其实就是这样的,再分裂了一次之后判断下一次分裂是否会超过N,如果不是那么就放心大胆地全部进行分裂(因为少了一个判断因此
//其运算速度会稍微快一些),如果会那么就引导到这里进行最后一次分裂
if((int)lNodes.size()>=N)
break;
}//遍历vPrevSizeAndPointerToNode并对其中指定的node进行分裂,直到刚刚达到或者超过要求的特征点个数
//这里理想中应该是一个for循环就能够达成结束条件了,但是作者想的可能是,有些子节点所在的区域会没有特征点,因此很有可能一次for循环之后
//的数目还是不能够满足要求,所以还是需要判断结束条件并且再来一次
//判断是否达到了停止条件
if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
bFinish = true;
}//一直进行nToExpand累加的节点分裂过程,直到分裂后的nodes数目刚刚达到或者超过要求的特征点数目
}//当本次分裂后达不到结束条件但是再进行一次完整的分裂之后就可以达到结束条件时
}// 根据兴趣点分布,利用4叉树方法对图像进行划分区域
// Retain the best point in each node
// 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个关键点响应值初始化最大响应值
float maxResponse = pKP->response;
//开始遍历这个节点区域中的特征点容器中的特征点,注意是从1开始哟,0已经用过了
for(size_t k=1;k<vNodeKeys.size();k++)
{
//更新最大响应值
if(vNodeKeys[k].response>maxResponse)
{
//更新pKP指向具有最大响应值的keypoints
pKP = &vNodeKeys[k];
maxResponse = vNodeKeys[k].response;
}
}
//将这个节点区域中的响应值最大的特征点加入最终结果容器
vResultKeys.push_back(*pKP);
}
//返回最终结果容器,其中保存有分裂出来的区域中,我们最感兴趣、响应值最大的特征点
return vResultKeys;
}