【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)

文章目录

  • 前言
  • 一、图像帧Frame的构造函数
  • 二、ExtractORB()的相关的关键函数
    • 1. 提取图像ORB特征点:ExtractORB()
    • 2. 仿函数,即重载括号运算符ORBextractor::operator() ()
    • 3. ORBextractor::ComputePyramid()函数
    • 4. ORBextractor::ComputeKeyPointsOctTree()
    • 5. ORBextractor::DistributeOctTree()
    • 6. ExtractorNode::DivideNode()函数
    • 7. static void computeOrientation()
    • 8. IC_Angle()函数
    • 9. static void computeDescriptors()
    • 10. static void computeOrbDescriptor()函数
  • 三、Frame构造函数的其他关键函数
    • 1. 计算去畸变函数的边界:void Frame::ComputeImageBounds()
    • 2. 确定矫正后特征点所在的网格:void Frame::AssignFeaturesToGrid()
  • 总结


前言

go on!纯梳理,重要的才列出了,不会把所有代码都展示。

关于源码中的问题,欢迎评论区沟通交流。


一、图像帧Frame的构造函数

Tracking.ccGrabImageMonocular()中的构造图像帧mCurrentFrame = Frame(),进入Frame.cc的单目帧构造函数Frame构造函数(双目和RGBD的图像帧构造函数的参数列表和提取数量有所差异)。

mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

Frame.cc:

Frame::Frame(const cv::Mat &imGray, 				// 灰度图
             const double &timeStamp, 				// 时间戳
             ORBextractor* extractor,				// ORB特征点提取器的句柄
             ORBVocabulary* voc, 					// ORB字典的句柄
             cv::Mat &K, 							// 内参
             cv::Mat &distCoef, 					// 畸变系数
             const float &bf, 						// 基线*焦距,用于计算视差
             const float &thDepth)					// 深度阈值,用于区分远近点
    :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
     mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)

ps:句柄:
在每个进程运算的过程中,有一个固定的地址(句柄)指向固定的区域A,区域A的值动态更新,这个值时刻记录这当前时刻对象在内存中的地址。
可理解为:知道句柄就可以知道对象在当前进程中的地址。
通常,同一对象在每次运行中的句柄值通常是不一样的。

step1:图像帧的ID自增:

    mnId=nNextId++;				// mnID:当前帧id;nNextID:下一帧id。在全局区被初始化

step2:计算图像金字塔参数(从ORB特征点提取器中获取);

step3:对单目图像进行特征点提取,得到关键点mvKeys和描述子mDescriptions;

step4:用内参对特征点进行去畸变操作UndistortKeyPoints()

注:以下两步一般是在第一帧或者是相机标定参数发生变化之后进行。

step5:计算去畸变后图像边界ComputeImageBounds(imGray)

step6:将特征点分配到网格中AssignFeaturesToGrid()

Ending。

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第1张图片

总结:
主要内容:
① 构建图像金字塔、
② 提取ORB特征点、
③ 进行特征点均匀化、
④ 计算特征点的主方向、
⑤ 高斯模糊、
⑥ 计算带方向的描述子、
⑦ 去畸变、
⑧ 计算去畸变后图像的边界、
⑨ 确定特征点所在的图像栅格


二、ExtractORB()的相关的关键函数

1. 提取图像ORB特征点:ExtractORB()

注:首次出现在Frame.cc的Frame构造函数中:

   ExtractORB(0,imGray);

功能:提取图像的ORB特征点,提取的关键点存放在mvKeys,描述子存放在mDescriptors。

/**
* @param[in] flag  0:左图  1:右图;
* 双目的时候,初始化、补充特征点/关键帧的时候会用到右图,在主要的跟踪过程中还是用左图
*/

void Frame::ExtractORB(int flag, const cv::Mat &im)
{
    if(flag==0)		// 判断左图还是右图
        // 运用仿函数,重载括号运算符ORBextractor::operator()
        (*mpORBextractorLeft)(im,				// 输入的图像
        					  cv::Mat(),		// 掩膜操作,没有用到
        					  mvKeys,			// 保存提取的关键点
        					  mDescriptors);	// 保存特征点的描述子
    else
        (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}

2. 仿函数,即重载括号运算符ORBextractor::operator() ()

***:真正意义上地实现提取ORB特征点,实现ExtractORB()函数的功能。

void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
                      OutputArray _descriptors)

步骤:
step1:检查图像有效性(可否在其上获取FAST角点):
判断是否为空、图像格式是否为所要求的单通道灰度图(不是灰度图的直接终止程序)。
注:转换为灰度图是为了让算法更加鲁棒,即使FAST角点也可以在彩色图上提取,且可以拓展算法的适用范围。

step2:构建图像金字塔:

	ComputePyramid(image);

step3:计算图像的ORB特征点,并将特征点均匀化(均匀化特征点可以提高位姿的计算精度):

    vector < vector<KeyPoint> > allKeypoints;
    ComputeKeyPointsOctTree(allKeypoints);

step4:遍历每层金字塔,累计金字塔的特征点个数,拷贝图像描述子到新的矩阵中,为后续计算描述子做准备。

step5:对图像进行高斯模糊:源码直接使用OpenCV中的高斯模糊函数进行处理。
高斯模糊(高斯平滑、正态分布模糊):
通常用它来减少图像噪声以及降低细节层次,以增强图像在不同比例大小下的图像效果;从数学的角度看,图像的高斯模糊过程就是图像与正态分布做卷积。对图像而言,高斯模糊就是一个低通滤波器。

高斯模糊 = 模糊 + 正态分布的二维密度函数(权重分布)

模糊:根据周围像素的灰度值而获得中间点的灰度值。

正态分布权重(解决周围像素平均值的权重问题):
在图形上,正态分布是一种钟形曲线,越接近中心,取值越大,越远离中心,取值越小。
计算平均值的时候,我们只需要将"中心点"作为原点,其他点按照其在正态曲线上的位置,分配权重,就可以得到一个加权平均值。
【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第2张图片
正态分布的密度函数又叫“高斯函数”,其二维形式为:
在这里插入图片描述
可用该式计算每个像素点的权重值。

故,计算高斯模糊的步骤为:
1)每个点乘以自己的权重值;
2)将这些值(如果3 * 3就是9个值,5 * 5就是25个值)加起来,就是中心点的高斯模糊的值;
3)如果是加权平均,还需要将权重之和为1。
重复上述步骤即可得到高斯模糊后的图像。
如果原图是彩色图片,可以对RGB三个通道分别做高斯模糊。

笔记来源:CSDN@ShaneHolmes

高斯模糊在ORB-SLAM2中的作用:模糊掉原本就不太明显的低质量ORB特征点。

step6:计算高斯模糊后的描述子

        computeDescriptors(workingMat, keypoints, desc, pattern);

step7:对非0层图像中的特征点的坐标恢复到第0层图像(原图像)的坐标系下
注:对于第0层的图像特征点,其坐标就不需要恢复

step8:通过引用参数keypoints,将allkeypoints中的所有特征点转存到输出的_keypoints

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第3张图片

3. ORBextractor::ComputePyramid()函数

功能:构建图像金字塔
首次出现:重载括号运算符仿函数中的step2

    ComputePyramid(image);

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第4张图片

	/**
     * 步骤:
     * 1. 先用resize()将图片缩放至sz大小,放入 mvImagePyramid[i];
     * 2. 接着用copyMakeBorder()扩展边界至wholeSize大小放入temp;
     * 3. EDGE_THRESHOLD是为了进行高斯模糊而预留的区域;
     * 4. 注意temp和mvImagePyramid[i]公用相同数据区,改变temp会改变mvImagePyramid[i]。
    */
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));	// 计算本层图像的像素尺寸大小
        Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);		// 获取全尺寸图像大小(如下图)
        Mat temp(wholeSize, image.type()), masktemp;			// temp为全尺寸图像,masktemp为掩膜(未使用),此时都空
        mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));	// 图像本身的区域

        // Compute the resized image 计算第0层以上重置后的图像
        if( level != 0 )
        {
            resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);		// 图像缩放,线性插值

            copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
                           BORDER_REFLECT_101+BORDER_ISOLATED);          // 采用OpenCV的扩充边函数进行扩充,在中心区域扩
        }
        else
        {	// 第0层的图层进行扩展边
            copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
                           BORDER_REFLECT_101);            				 // 在原图上直接扩展
        }
    }
}

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第5张图片
图源:@计算机视觉life

4. ORBextractor::ComputeKeyPointsOctTree()

功能:使用四叉树计算图像金字塔每一层的特征点,并均匀化,储存在allKeypoints里
首次出现:重载括号运算符仿函数中的step3

    ComputeKeyPointsOctTree(allKeypoints);
	/**
	 * 步骤:
	 * 1.先是计算提取关键点的边界,太边缘的地方放弃提取关键点;
	 * 2.将图像分割为W*W的小图片,遍历这些分割出来的小图片并提取其关键点;
	 * 3.将提取的关键点交给DistributeOctTree()利用四叉树以及非极大值抑制算法进行筛选;
	 * 4.计算关键点的相关信息:像素位置,patch尺度,方向,所在高斯金字塔层数;
	 * 注意:在分割图片的时候,小窗之间是有重叠的
	*/
	void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints){}

step1:调整图像层数,设定小窗口(栅格)的尺寸W = 30;

step2:对每一层图像做处理,设定边界,划分网格
【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第6张图片
图源:@计算机视觉life

	// 计算每一层可以提取特征点的有效图像边界,结合上图理解(中间的灰色区域+绿色区域)
	 const int minBorderX = EDGE_THRESHOLD-3;		// 3:提取FAST角点时会创建一个半径为3像素的圆,故预留3pixels的空间
	 const int minBorderY = minBorderX;				// 不管图层是正方形还是长方形,此处的最小边界X=Y
	 const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
	 const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;
	
	 vector<cv::KeyPoint> vToDistributeKeys;		// 存储待筛选的ORB特征点
	 vToDistributeKeys.reserve(nfeatures*10);		// 扩容,一般都会过量采集,所以要预留大一点的空间
	
	 // 计算有效图像的尺寸(个人吐槽:这几个函数中都在做着这些事情,反复地写着这些)
	 const float width = (maxBorderX-minBorderX);
	 const float height = (maxBorderY-minBorderY);
	
	 const int nCols = width/W;						// 划分小网格后,每层图像可以划分为多少列网格,强制截断向下取整
	 const int nRows = height/W;					// 每层图像可以划分成多少行网格
	 const int wCell = ceil(width/nCols);			// 每层图像宽度/上述假设的小网格尺寸所划分出来的列数=实际划分网格的宽度
	 const int hCell = ceil(height/nRows);			// 每层图像高度/上述假设的小网格尺寸所划分出来的行数=实际划分网格的高度

step3:开始遍历图像网格,提取网格内的FAST角点,转换FAST角点坐标,并存储。

	// 从行开始遍历
	for(int i=0; i<nRows; i++)
        {
            const float iniY =minBorderY+i*hCell;	// 第一个网格Y方向的上边界
            float maxY = iniY+hCell+6;		// 第一个网格Y方向的下边界,6:网格边界像素提取FAST角点时预留的像素点,3+3=6

            if(iniY>=maxBorderY-3)					// 如果第一个网格Y方向的上边界超过了图像有效边界,跳过
                continue;
            if(maxY>maxBorderY)						// 如果第一个网格Y方向的下边界超过了图像有效边界,直接置等
                maxY = maxBorderY;

			// 开始列的遍历
            for(int j=0; j<nCols; j++)
            {
                const float iniX =minBorderX+j*wCell;	// 第一个网格X方向的左边界
                float maxX = iniX+wCell+6;				// 第一个网格X方向的右边界
                if(iniX>=maxBorderX-6)					// 一格X左边界超过有效边界,跳过;
                										// 应该是减3吧,无伤大雅,少几个特征点而已
                    continue;				
                if(maxX>maxBorderX)						// 一格X右边界超过有效边界,置等
                    maxX = maxBorderX;

                vector<cv::KeyPoint> vKeysCell;			// 存储网格中的特征点
                FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
                     vKeysCell,iniThFAST,true);			// 用OpenCV中的函数提取FAST角点

                if(vKeysCell.empty())					// 若没检测到FAST角点,则用更低的检测阈值来进行提取
                {
                    FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
                         vKeysCell,minThFAST,true);
                }
				
				/**
				* 从这一步的开始到目前为止,一直是在【网格】的角度上描述角点
				* 现在需要将提取到的FAST角点从【网格】视角转换到【当前层图像有效边界(灰色+绿色区域)】视角下的坐标
				*/
                if(!vKeysCell.empty())					// 如果提取到了角点,遍历所有角点,转换为当前层图像下的坐标
                {
                    for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
                    {
                        (*vit).pt.x+=j*wCell;
                        (*vit).pt.y+=i*hCell;
                        vToDistributeKeys.push_back(*vit);
                    }
                }
            }
        }

step4:对每一层所提取到的FAST角点用四叉树和非极大值抑制算法均匀分布和筛选(核心)

keypoints = DistributeOctTree(vToDistributeKeys, 					// 当前层等待筛选的特征点,此时特征点还是在【有效边界】
							  minBorderX, maxBorderX,				// 当前层的图像边界,而此时的坐标却是【边缘边界】下的
                              minBorderY, maxBorderY,				// 【边缘边界】:上图最外围的边界
                              mnFeaturesPerLevel[level], 			// 每层图像中需要提取/希望保留的特征点数目
                              level);

step5:计算每层图像的尺度缩放后的PATCH

const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level]; 

源码一开始的PATCH_SIZE是对于底层的初始图像(原图)来说的,现在要根据当前图层的尺度缩放倍数进行缩放,其大小和特征点的方向计算有关系。每一层的图像是已经缩小的,但为保持一致性,在本层提取到的ORB特征点、计算描述子等情况时需要依据的PATCH应该还是PATCH_SIZE,所以将ORB特征点恢复到原始图像上时的代表特征点的尺度信息需要放大,恢复原来大小。

step6:遍历筛选后的特征点,并将其坐标恢复到【边缘边界图像】坐标系下,即将坐标统一成当前金字塔层的坐标

        const int nkps = keypoints.size();
        for(int i=0; i<nkps ; i++)
        {
            keypoints[i].pt.x+=minBorderX;
            keypoints[i].pt.y+=minBorderY;
            keypoints[i].octave=level;
            keypoints[i].size = scaledPatchSize;
        }

step7:遍历每一层的特征点,计算其方向信息,此时还是分层计算的(核心)

computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第7张图片

5. ORBextractor::DistributeOctTree()

功能:使用四叉树法对一个图像金字塔图层中的特征点进行平均和分配(只是对于单层图像而言,个人感觉“分配”指的是确定这个特征点是在某一个具体的节点上,而不是分过去),返回值是一个保存有特征点的vector容器
首次出现:ComputeKeyPointsOctTree()中的step4

        keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
                                      minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);

整体思路(来源:知乎@SLAM算法&技术):

  1. 如果图片的宽度比较宽,就先把分成左右w/h份。一般的640×480的图像开始的时候只有一个node。
  2. 如果node里面的点数>1,把每个node分成四个node,如果node里面的特征点为空,就不要了,删掉。
  3. 新分的node的点数>1,就再分裂成4个node。
  4. 如此,一直分裂。
  5. 终止条件为:node的总数量> [公式] ,或者无法再进行分裂。
  6. 最后从每个node里面选择一个质量最好的FAST点。
		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)

step1:根据宽高比确定初始节点的数目,通常为1或2

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第8张图片

    const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));		// 计算初始节点。如果nIni小于0.5,会直接等于0,后面会出错
    const float hX = static_cast<float>(maxX-minX)/nIni;					// 每个初始节点在X方向上占多少个像素

step2:生成初始提取器节点,将根节点分为4个区域UL、UR、BL、BR,设置其边界顶点
注:这里和提取FAST角点区域相同,都是【有效边界区域】,特征点坐标从0开始

step3:将特征点分配到子特征点提取器中

	// 遍历vToDistributeKeys,并将存储在其中的带有坐标的特征点传给子提取器vpIniNodes
    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坐标,将所有的关键点分配到指定节点下的vKeys容器中去。

step4:遍历提取器节点链表,标记不可以再分裂的节点,删除那些没有分配到特征点的节点

现在lNodes中只放有根节点,变量根节点,如果根节点中关键点个数为1,那么将这个节点的bNoMore设置为true,表示这个节点不能再分裂了。如果为空,那么就删除这个节点。

step5:根据兴趣点分布,利用四叉树方法对图像进行划分区域(参考:CSDN@认真的虎)

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第9张图片

// 遍历双向列表中所有的提取器节点,进行分解或者保留
 while(lit!=lNodes.end())
        {	// 如果该节点只有一个特征点,那不用再进行细分了,直接保留
            if(lit->bNoMore)
            {	
                // If node only contains one point do not subdivide and continue
                lit++;
                continue;
            }
            else
            {	// 如果不止一个特征点,那就进行细分,1分为4
                // If more than one point, subdivide
                ExtractorNode n1,n2,n3,n4;
                lit->DivideNode(n1,n2,n3,n4);	//DivideNode的作用是将node分成均匀4份, 并把里面的特征点确定给4个子node

                // Add childs if they contain points
                /**
                * 如果划分出来的子区域中有特征点,就将代表该区域的节点添加到特征点提取器INodes头部
                * 有特征点就可以放进去
                * 如果特征点数量大于0,就放进特征点提取器INodes头部;
                * 如果特征点数量大于1,同时放进vSizeAndPointerToNode中,待分裂计数+1
                */
                if(n1.vKeys.size()>0)
                {
                    lNodes.push_front(n1);                    
                    if(n1.vKeys.size()>1)
                    {
                        nToExpand++;
                        vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
                        lNodes.front().lit = lNodes.begin();			// 记录node里的位置,所以不知道这个有什么用
                    }
                }
     			// ...(n2、3、4类推)
                lit=lNodes.erase(lit);		// 母节点分裂后,就删掉自己
                continue;
            }
        }     
          

if:当前节点数超过要求的特征点数或者当前所有节点只包含一个特征点(程序把这个条件描述为:分裂前和分裂后的总节点数一样),就停止分裂,置停止位为true。

else if:两次划分后的节点数大于所要求的特征点数目时,就在第二次划分的时候,划分至刚好达到要求或者超过要求的特征点数目即可。

// 待分裂的节点数vSizeAndPointerToNode分裂后,一般会增加nToExpand*3,如果增加nToExpand*3后大于N,则进入此分支。
 else if(((int)lNodes.size()+nToExpand*3)>N)
        {

            while(!bFinish)
            {

                prevSize = lNodes.size();

				// 浅拷贝那些还可以分裂的节点信息
                vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
                vSizeAndPointerToNode.clear();
				
				// 对需要划分的节点进行排序,对pair对的第一个元素进行排序,默认是从小到大排序,即特征点的多少
				// 优先分裂特征点多的节点,使得特征点密集的区域保留更少的特征点
				// vPrevSizeAndPointerToNode.second为对应的节点
                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 points
                    // 跟前面执行一样的操作
                    if(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((int)lNodes.size()>=N)		// 如果刚刚好就停止分裂、跳出循环
                        break;
					// ...(略)

step7:保留每个区域中响应值最大的一个特征点

step8:返回每一层图像每个区域中最终最感兴趣、响应值最大的特征点

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第10张图片

6. ExtractorNode::DivideNode()函数

功能:将node四等分,并把里面的特征点确定位于某一个子节点中,并计数
首次出现:DistributeOctTree()函数的划分节点。

   lit->DivideNode(n1,n2,n3,n4);
void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)

此处可能有一个bug:特征点的坐标是在【有效边界图像】下的,而节点区域的坐标是在【边缘边界图像】下,而作者直接作比较。

    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);
            else
                n3.vKeys.push_back(kp);
        }
        else if(kp.pt.y<n1.BR.y)
            n2.vKeys.push_back(kp);
        else
            n4.vKeys.push_back(kp);
    }

    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;
}

7. static void computeOrientation()

这是一个不属于任何类的全局静态函数,static修饰符限定其只能够被本文件中的函数调用。

功能:计算特征点的方向
首次出现:ComputeKeyPointsOctTree()最后一步,计算特征点方向

    computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
// umax 为之前计算出来的一个数组长度为16的数值
// umax[16]中所有数据,依次是:15 15 15 15 14 14 14 13 13 12 11 10 9 8 6 3
static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax)
{
    for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
         keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
    {
        keypoint->angle = IC_Angle(image, keypoint->pt, umax);
    }
}

转入IC_Angle()函数。

8. IC_Angle()函数

功能:用于计算特征点的方向,这里是返回角度作为方向。
一个不属于任何类的全局静态函数,static修饰符限定其只能够被本文件中的函数调用。
这里运用的是前面文章中提到的灰度质心法计算FAST关键点方向。

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第11张图片

在一个圆域中算出m10(x方向)和m01(y方向),先算出中间红线的m10,然后在平行于x轴算出m10和m01,一次计算相当于图像中的同个颜色的两个line。

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第12张图片
图源:博客园@h_立青人韦

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第13张图片
图源:@计算机视觉life

static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
{
    int m_01 = 0, m_10 = 0;		// 图像的矩,前者y坐标加权,后者x坐标加权,一阶矩

    const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));		// 获取特征点的灰度值

    // Treat the center line differently, v=0						// v = 0中心线比较特殊,单独计算 
    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)		// 按着上图理解,遍历u轴
        m_10 += u * center[u];										// v = 0,即y方向加权为0,无m_01

    // Go line by line in the circular patch						// v ≠ 0 的像素灰度值计算
    int step = (int)image.step1();		// step1表示这个图像一行包含的字节总数,后面遍历的时候增量乘step表示像素增量。
    for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
    {
        // Proceed over the two lines								// 同时算两行
        int v_sum = 0;
        int d = u_max[v];											// 获取每行像素横坐标的最大范围
        for (int u = -d; u <= d; ++u)
        {
        	// val_plus:在中心线下方x=u时的的像素灰度值
            // val_minus:在中心线上方x=u时的像素灰度值
            int val_plus = center[u + v*step], val_minus = center[u - v*step];

		// 对于某次待处理的两个点:m_10 = Σ x*I(x,y) =  x*I(x,y) + x*I(x,-y) = x*(I(x,y) + I(x,-y))
        // 对于某次待处理的两个点:m_01 = Σ y*I(x,y) =  y*I(x,y) - y*I(x,-y) = y*(I(x,y) - I(x,-y))
            v_sum += (val_plus - val_minus);
            m_10 += u * (val_plus + val_minus);
        }
        m_01 += v * v_sum;
    }
    return fastAtan2((float)m_01, (float)m_10);
}

9. static void computeDescriptors()

一个不属于任何类的全局静态函数,static修饰符限定其只能够被本文件中的函数调用。

功能:计算高斯模糊后图像的描述子,其计算所需的点对分布采用高斯分布
首次出现:括号运算符仿函数的倒数几步,计算高斯模糊后图像的描述子

        computeDescriptors(workingMat,		// 高斯模糊后的图像
         				   keypoints, 		// 当前层中的特征点集合
         				   desc,			// 存储计算后得到的描述子 
         				   pattern);		// 随机采样的点集
static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors,
                               const vector<Point>& pattern)
{
    descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);
	// 开始遍历每一个特征点
    for (size_t i = 0; i < keypoints.size(); i++)
        computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i));
}

转入static void computeOrbDescriptor()函数。

10. static void computeOrbDescriptor()函数

一个不属于任何类的全局静态函数,static修饰符限定其只能够被本文件中的函数调用。

功能:计算每个特征点的描述子,在该函数中用前面算出来的特征点方向来确定描述子的旋转主方向(theta θ),使其具有较好的旋转不变性,结合特征点主方向计算出来的BRIEF描述子称为“Steer BRIEF”。

核心思路:计算将随机选取的点点集坐标系的X轴方向旋转到和特征点主方向重合,然后得到随机“相对点集”中某一个idx对应点的灰度值,最后再与特征点的灰度值相比较,计算出二进制描述子。
【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第14张图片
图源:@计算机视觉life

Steer BRIEF如何确定旋转的主方向:
【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第15张图片
图源:@计算机视觉life

static void computeOrbDescriptor(const KeyPoint& kpt,
                                 const Mat& img, const Point* pattern,
                                 uchar* desc)
{
    float angle = (float)kpt.angle*factorPI;					// 弧度制表示特征点
    float a = (float)cos(angle), b = (float)sin(angle);			// 计算前面角度的余弦值与正弦值

    const uchar* center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));		// 获取图像中心的灰度值
    const int step = (int)img.step;

	// 定义宏:
	// 给一个序号idx,从pattern表中找到一点pattern[idx],然后用上面的公式计算这个点经过旋转θ后的点坐标,然后得到这个点的灰度值
	// y'* step + x',与上面的IC_Angle()函数中计算对应像素点灰度值的操作一样,都是通过center[X]来进行检索
    #define GET_VALUE(idx) \
        center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \
               cvRound(pattern[idx].x*a - pattern[idx].y*b)]			

	// 一个brief描述子由32*8=256位组成,通常分成32行8列来表示
	// 其中每一位是来自于两个像素点灰度的直接比较,所以每次比较出8bit结果,需要16个随机点
    for (int i = 0; i < 32; ++i, pattern += 16)
    {
        int t0, t1, val;
        t0 = GET_VALUE(0); t1 = GET_VALUE(1);
        val = t0 < t1;
        t0 = GET_VALUE(2); t1 = GET_VALUE(3);
        val |= (t0 < t1) << 1;
		// ...
        desc[i] = (uchar)val;
    }

    #undef GET_VALUE
}

<<是移位符号,比如val |= (t0 < t1) << 1,就是先判断t0


三、Frame构造函数的其他关键函数

1. 计算去畸变函数的边界:void Frame::ComputeImageBounds()

reference:CSDN@文科升、@不能再吃了OvO

void Frame::ComputeImageBounds(const cv::Mat &imLeft)
{
    if(mDistCoef.at<float>(0)!=0.0)
    {
        //创建4*2的矩阵
        /**
         * | 0.0   0.0  |
         * | cols  0.0  |
         * | 0.0   rows |
         * | cols  rows |
         * 其中cols为imLeft的列数,rows为imLeft的行数。其实这个矩阵的4个点对也就是图像的4个边角点坐标
         **/
        cv::Mat mat(4,2,CV_32F);
        mat.at<float>(0,0)=0.0; mat.at<float>(0,1)=0.0;
        mat.at<float>(1,0)=imLeft.cols; mat.at<float>(1,1)=0.0;
        mat.at<float>(2,0)=0.0; mat.at<float>(2,1)=imLeft.rows;
        mat.at<float>(3,0)=imLeft.cols; mat.at<float>(3,1)=imLeft.rows;

        // Undistort corners		// 其他操作和特征点去畸变一样
        mat=mat.reshape(2);
        cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
        mat=mat.reshape(1);
        // ...

【ORB-SLAM2源码梳理4】图像帧Frame构造函数及其关键函数(Frame.cc与ORBextractor.cc)_第16张图片
图源:CSDN@不能再吃了OvO

是的,没错,如果已经完成特征点去畸变后,就对图像边界的4个点去畸变。

2. 确定矫正后特征点所在的网格:void Frame::AssignFeaturesToGrid()

void Frame::AssignFeaturesToGrid()
{
	// 为每一个栅格预留0.5N的空间。但是不知道为啥是0.5N的空间
    int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
    
    // 遍历所有栅格,重设每一个栅格的空间容量
    for(unsigned int i=0; i<FRAME_GRID_COLS;i++)		
        for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
            mGrid[i][j].reserve(nReserve);

	// 遍历所有特征点,按照特征点所在的位置,将索引记录到网格的mGrid中
    for(int i=0;i<N;i++)
    {
        const cv::KeyPoint &kp = mvKeysUn[i];
        int nGridPosX, nGridPosY;
        if(PosInGrid(kp,nGridPosX,nGridPosY))
            mGrid[nGridPosX][nGridPosY].push_back(i);
    }
}

总结

这篇太长了,累了。
终于写完了,撒花。

你可能感兴趣的:(ORB-SLAM2,VSLAM入门,slam,orb)