ORB-SLAM2源码学习（一）

总结：mono_kitti.cc的框架分为以下步骤：

1.LoadImages 加载图片和时间戳

2.实例化SLAM对象，这里初始化了很多线程

3.SLAM.TrackMonocular(im,tframe) 是代码的核心，特征点提取以及均匀化和描述子计算都在这里实现，同时还有整个系统是如何实现跟踪的，还有就是关键帧的选择方式

4.SLAM.shutdown() 关闭所有线程

5.记录跟踪图片的时间

6.将相机轨迹保存

系统框架梳理

mono_kitti.cc开始代码

首先知道输入的四个参数 argv[0] argv[1] argv[2] argv[3]

./Examples/Monocular/mono_kitti   Vocabulary/ORBvoc.txt   Examples/Monocular/KITTI00-02.yaml   dataset/sequences/00

step1 加载图像

main()开始定义了两个容器分别存放每张图片的路径和时间戳

vector<string> vstrImageFilenames; //每张图片存放路径
vector<double> vTimestamps;        //每张图片时间戳

然后执行LoadImags函数 , 函数作用：读取数据集下的图片路径和对应的时间戳并将读取结果放到容器中

LoadImages(string(argv[3]), vstrImageFilenames, vTimestamps);

重点看函数定义：

// 获取图像序列中每一张图像的访问路径和时间戳
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{// step 1 读取时间戳文件ifstream fTimes;        //定义输入流读取文件中的时间戳string strPathTimeFile = strPathToSequence + "/times.txt";//此时 strPathTimeFile = dataset/sequences/00/times.txt//c_str是string类的一个函数，可以把string类型变量转换成char*变量//open()要求的是一个char*字符串//当文件名是string时需要转换，当文件名是字符数组型时就不需要此转换fTimes.open(strPathTimeFile.c_str());while(!fTimes.eof())   //eof()函数判断文件夹是否读到最后 end of file{string s;getline(fTimes,s);  //自动读取下一行时间戳文件   s是读取的结果，以times.txt第一行为例 s = 0.000000e+00// 如果该行字符串不是空的，就写进流if(!s.empty())  {//stringstream 将string类型的 s 转为double类型stringstream ss;ss << s;      double t;ss >> t;// 容器中保存了所有图片的时间戳vTimestamps.push_back(t);}}// step 2 使用左目图像, 生成左目图像序列中的每一张图像的文件名string strPrefixLeft = strPathToSequence + "/image_0/";const int nTimes = vTimestamps.size();vstrImageFilenames.resize(nTimes);  //图像的数量要和时间戳一一对应for(int i=0; i<nTimes; i++){stringstream ss;//std::setw ：需要填充多少个字符,默认填充的字符为 ' '    空格//std::setfill：设置std::setw将填充什么样的字符，如:std::setfill('*')//ss总共为6位，i之外的前边几位用0来填充，得到的结果为000001- 000099之类ss << setfill('0') << setw(6) << i;  //填6个0,如果来了个i，则代替末尾0的位置，也就是总共有6位，末尾是i，其他还是0vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png";      //图片路径dataset/sequences/00/image_0/ 000000 .png}
}

main() 继续执行，定义变量nImages vstrImageFilenames容器中图片的个数

int nImages = vstrImageFilenames.size(); //有nImages个图片

step2 加载SLAM系统（重点）

之后实例化一个SLAM对象

ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

先看System的构造函数：

System::System(const string &strVocFile,                 //词典文件路径const string &strSettingsFile,              //配置文件路径const eSensor sensor,                       //传感器类型const bool bUseViewer):                      //是否使用可视化界面mSensor(sensor),                             //初始化传感器类型mpViewer(static_cast<Viewer*>(NULL)),       //空。。。对象指针？  视觉SLAMch6 g2o代码中使用过，将指向基类的指针转化为指向派生类的指针mbReset(false),                           //无复位标志mbActivateLocalizationMode(false),           //没有这个模式转换标志mbDeactivateLocalizationMode(false)     //没有这个模式转换标志
{

构造函数主要分为：

①输出欢迎信息和传感器类型;

②打开配置文件(.yaml格式)，使用cv::FileStorage进行读操作;

//!2. 打开YAML文件进行读操作
cv::FileStorage fsSettings(strSettingsFile.c_str(),     //将配置文件名转换成为字符串cv::FileStorage::READ);      //只读
//如果打开失败，就输出调试信息
if(!fsSettings.isOpened())
{cerr << "Failed to open settings file at: " << strSettingsFile << endl;//然后退出exit(-1);
}

③加载ORB词包

//! 加载ORB词包
cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
//建立一个新的ORB字典
//使用无参构造方法构造对象，使用System类中定义的ORBVocabulary类的类指针*mpVocabulary 在堆区存储对象的位置
//ORBVocabulary类来源于DBoW2中，能够快速稳定计算两帧图像的相似程度
mpVocabulary = new ORBVocabulary();
//使用指针调用类函数加载词包，并返回bool值判断是否加载成功
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
//如果加载失败，就输出调试信息
if(!bVocLoad)
{cerr << "Wrong path to vocabulary. " << endl;cerr << "Falied to open at: " << strVocFile << endl;//然后退出exit(-1);
}
//否则则说明加载成功
cout << "Vocabulary loaded!" << endl << endl;
//!完成词袋的提取

④利用词袋初始化关键帧数据库（用于重定位和回环检测），存入关键帧数据库指针

 //!4. 利用词袋初始化关键帧数据库（用于重定位和回环检测），存入关键帧数据库指针mpKeyFrameDatabase
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

⑤创建一个Map类，存储指向所有关键帧和地图点的指针mpMap

//!5. 创建一个Map类，存储指向所有关键帧和地图点的指针mpMap
mpMap = new Map();

⑥创建两个窗口，帧绘制器和地图绘制器mpFrameDrawer 和 mpMapDrawer

//!6. 创建两个窗口，帧绘制器和地图绘制器mpFrameDrawer  mpMapDrawer
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

⑦初始化线程Tracking

//!7. 初始化追踪线程
mpTracker = new Tracking(this,         //现在还不是很明白为什么这里还需要一个this指针  TODO  mpVocabulary,             //字典mpFrameDrawer,              //帧绘制器mpMapDrawer,              //地图绘制器mpMap,                       //地图mpKeyFrameDatabase,         //关键帧地图strSettingsFile,             //设置文件路径mSensor);                   //传感器类型iomanip

同样重点看它的构造函数

///构造函数
Tracking::Tracking(System *pSys,                       //系统实例ORBVocabulary* pVoc,                //BOW字典FrameDrawer *pFrameDrawer,          //帧绘制器MapDrawer *pMapDrawer,              //地图点绘制器Map *pMap,                          //地图句柄KeyFrameDatabase* pKFDB,            //关键帧产生的词袋数据库const string &strSettingPath,       //配置文件路径const int sensor):                  //传感器类型mState(NO_IMAGES_YET),                              //当前系统还没有准备好mSensor(sensor),                                mbOnlyTracking(false),                              //处于SLAM模式mbVO(false),                                        //当处于纯跟踪模式的时候，这个变量表示了当前跟踪状态的好坏mpORBVocabulary(pVoc),          mpKeyFrameDB(pKFDB), mpInitializer(static_cast<Initializer*>(NULL)),     //暂时给地图初始化器设置为空指针mpSystem(pSys), mpViewer(NULL),                                     //注意可视化的查看器是可选的，因为ORB-SLAM2最后是被编译成为一个库，所以对方人拿过来用的时候也应该有权力说我不要可视化界面（何况可视化界面也要占用不少的CPU资源）mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0)                               //恢复为0,没有进行这个过程的时候的默认值
{// Step 1 从配置文件中加载相机参数cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);float fx = fSettings["Camera.fx"];float fy = fSettings["Camera.fy"];float cx = fSettings["Camera.cx"];float cy = fSettings["Camera.cy"];//          |fx  0   cx|// K =   |0   fy  cy|//          |0   0   1 |//构造相机内参矩阵cv::Mat K = cv::Mat::eye(3,3,CV_32F);   //得到的是对角线为1 其余为0的三阶矩阵K.at<float>(0,0) = fx;K.at<float>(1,1) = fy;K.at<float>(0,2) = cx;K.at<float>(1,2) = cy;//内参矩阵赋值给成员变量K.copyTo(mK);// 图像矫正系数// [k1 k2 p1 p2 k3]   部分相机的k3是0cv::Mat DistCoef(4,1,CV_32F);DistCoef.at<float>(0) = fSettings["Camera.k1"];DistCoef.at<float>(1) = fSettings["Camera.k2"];DistCoef.at<float>(2) = fSettings["Camera.p1"];DistCoef.at<float>(3) = fSettings["Camera.p2"];const float k3 = fSettings["Camera.k3"];//有些相机的畸变系数中会没有k3项if(k3!=0){DistCoef.resize(5);DistCoef.at<float>(4) = k3;}//畸变参数赋值给成员变量DistCoef.copyTo(mDistCoef);// 双目摄像头baseline * fx 50mbf = fSettings["Camera.bf"];//相机每秒处理的帧数float fps = fSettings["Camera.fps"];if(fps==0)fps=30;// Max/Min Frames to insert keyframes and to check relocalisation//用于插入关键帧和检查重新定位的最大/最小帧mMinFrames = 0;mMaxFrames = fps;//输出cout << endl << "Camera Parameters: " << endl;cout << "- fx: " << fx << endl;cout << "- fy: " << fy << endl;cout << "- cx: " << cx << endl;cout << "- cy: " << cy << endl;cout << "- k1: " << DistCoef.at<float>(0) << endl;cout << "- k2: " << DistCoef.at<float>(1) << endl;if(DistCoef.rows==5)cout << "- k3: " << DistCoef.at<float>(4) << endl;cout << "- p1: " << DistCoef.at<float>(2) << endl;cout << "- p2: " << DistCoef.at<float>(3) << endl;cout << "- fps: " << fps << endl;// 1:RGB 0:BGR      好像Camera.RGB的值都是1？？？ int nRGB = fSettings["Camera.RGB"];mbRGB = nRGB;if(mbRGB)cout << "- color order: RGB (ignored if grayscale)" << endl;elsecout << "- color order: BGR (ignored if grayscale)" << endl;// Load ORB parameters// Step 2 加载ORB特征点有关的参数,并新建特征点提取器// 每一帧提取的特征点数 1000int nFeatures = fSettings["ORBextractor.nFeatures"];// 图像建立金字塔时的变化尺度 1.2float fScaleFactor = fSettings["ORBextractor.scaleFactor"];// 尺度金字塔的层数 8int nLevels = fSettings["ORBextractor.nLevels"];// 提取fast特征点的默认阈值 20int fIniThFAST = fSettings["ORBextractor.iniThFAST"];// 如果默认阈值提取不出足够fast特征点，则使用最小阈值 8int fMinThFAST = fSettings["ORBextractor.minThFAST"];// !tracking过程都会用到mpORBextractorLeft作为特征点提取器mpORBextractorLeft = new ORBextractor(nFeatures,      //参数的含义还是看上面的注释吧fScaleFactor,nLevels,fIniThFAST,fMinThFAST);// 如果是双目，tracking过程中还会用用到mpORBextractorRight作为右目特征点提取器if(sensor==System::STEREO)mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);// 在单目初始化的时候，会用mpIniORBextractor来作为特征点提取器if(sensor==System::MONOCULAR)mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);cout << endl  << "ORB Extractor Parameters: " << endl;cout << "- Number of Features: " << nFeatures << endl;cout << "- Scale Levels: " << nLevels << endl;cout << "- Scale Factor: " << fScaleFactor << endl;cout << "- Initial Fast Threshold: " << fIniThFAST << endl;cout << "- Minimum Fast Threshold: " << fMinThFAST << endl;if(sensor==System::STEREO || sensor==System::RGBD){// 判断一个3D点远/近的阈值 mbf * 35 / fx//ThDepth其实就是表示基线长度的多少倍mThDepth = mbf*(float)fSettings["ThDepth"]/fx;cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl;}if(sensor==System::RGBD){// 深度相机disparity转化为depth时的因子mDepthMapFactor = fSettings["DepthMapFactor"];if(fabs(mDepthMapFactor)<1e-5)mDepthMapFactor=1;elsemDepthMapFactor = 1.0f/mDepthMapFactor;}}

东西比较多，核心的地方是提取ORB特征点

mpORBextractorLeft = new ORBextractor(nFeatures,      //参数的含义还是看上面的注释吧fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

继续看它的构造函数。提取ORB特征点的时候使用图像金字塔使其具有尺度不变性，使用灰度质心法具有旋转不变性。

    //存储每层图像缩放系数的vector调整为符合图层数目的大小mvScaleFactor.resize(nlevels);  //存储这个sigma^2，其实就是每层图像相对初始图像缩放因子的平方mvLevelSigma2.resize(nlevels);//对于初始图像，这两个参数都是1mvScaleFactor[0]=1.0f;mvLevelSigma2[0]=1.0f;//然后逐层计算图像金字塔中图像相当于初始图像的缩放系数 for(int i=1; i<nlevels; i++)  {//其实就是这样累乘计算得出来的mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor;//原来这里的sigma^2就是每层图像相对于初始图像缩放因子的平方mvLevelSigma2[i]=mvScaleFactor[i]*mvScaleFactor[i];}//接下来的两个向量保存上面的参数的倒数mvInvScaleFactor.resize(nlevels);mvInvLevelSigma2.resize(nlevels);for(int i=0; i<nlevels; i++){mvInvScaleFactor[i]=1.0f/mvScaleFactor[i];mvInvLevelSigma2[i]=1.0f/mvLevelSigma2[i];}//!开始图像金字塔分配特征点数目//调整图像金字塔vector以使得其符合设定的图像层数mvImagePyramid.resize(nlevels);//每层需要提取出来的特征点个数，这个向量也要根据图像金字塔设定的层数进行调整，所用层特征点加起来等于nfeaturesmnFeaturesPerLevel.resize(nlevels);//图片的缩放因子s（小于1）float factor = 1.0f / scaleFactor;//第0层图像应该分配的特征点数量    N*(1-s) / (1-pow(s,n) )float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));   //第x层分配的特征点数量   pow(s,x) *  N*(1-s) / (1-pow(s,n) )//累计有多少特征点已经被分配int sumFeatures = 0;//开始逐层计算要分配的特征点个数，顶层图像除外（看循环后面）for( int level = 0; level < nlevels-1; level++ )    //如果nlevels=8，这里只分配了前七层的特征点数量，最后一层没有分配{//分配 cvRound : 返回个参数最接近的整数值mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);   //cvRound 取整//累计sumFeatures += mnFeaturesPerLevel[level];//乘系数nDesiredFeaturesPerScale *= factor;    //特征点数量也会根据缩放因子等比例分配}//由于前面的特征点个数取整操作，可能会导致剩余一些特征点个数没有被分配，所以这里就将这个余出来的特征点分配到最高的图层中mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);//!完成了对每层图像金字塔分配特征点数目的操作

公式推导：

设第0层面积（像素个数）为S0，每层的缩放因子s（小于1），总面积S为

$S = S_0 \times s^0 + S_0 \times s^1 +......+S_0 \times s^{n-1} = S_0 \times \frac{1-s^n}{1-s}$

每次提取的特征点数量是 N,那么单位面积提取的特征点数量为

$\frac{N}{S} = \frac{N \times (1-s)}{S_0 \times (1-s^n)}$

因此第0层提取到的特征点为：

$N_0 = S_0 \times \frac{N \times (1-s)}{S_0 \times (1-s^n)} = \frac{N \times (1-s)}{1-s^n}$

第x层提取到的特征点为：

$N_x = s^x \times \frac{N \times (1-s)}{1-s^n}$

总结：每一层提取的特征点数量 = 前一层提取到的数量 X 缩放因子s

然后读取描述子256对点放到pattren中（读取描述子坐标）

//成员变量pattern的长度，也就是点的个数，这里的512表示512个点
const int npoints = 512;
//获取用于计算BRIEF描述子的随机采样点点集头指针
//注意到pattern0数据类型为Points*,bit_pattern_31_是int[]型，所以这里需要进行强制类型转换
const Point *pattern0 = (const Point *)bit_pattern_31_; //bit_pattern_31_就是存放描述子的大数组
//使用std::back_inserter的目的是可以快覆盖掉这个容器pattern之前的数据
//其实这里的操作就是，将在全局变量区域的、int格式的随机采样点以cv::point格式复制到当前类对象中的成员变量中（关键）
std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern)); //pattern是类ORBextractor的成员变量

这里参考描述子提取过程，bit_pattern_31_ 这个数组里有256行

BRIEF描述子由32* 8位组成，每一位都是两个像素点（一行）比较像素的结果，每比较一个8bit需要8行的点。

以bit_pattern_31_第一行为例（8 -3 9 5），以某个特征点为中心，构建一个31*31的块，比较（8，-3）和（9，5）这两个地方的像素，大于的话记为1，反之记为0，然后比较8行得到第一组的描述子，继续比较得到32*8的描述子。

计算灰度质心法圆形区域边界

//This is for orientation//下面的内容是和特征点的旋转计算有关的//预先计算圆形patch中行的结束位置//+1中的1表示那个圆的中间行umax.resize(HALF_PATCH_SIZE + 1);//cvFloor返回不大于参数的最大整数值(向下取整)，cvCeil返回不小于参数的最小整数值(向上取整)，cvRound则是四舍五入int v,     //循环辅助变量v0,     //辅助变量vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);    //计算圆的最大行号，+1应该是把中间行也给考虑进去了  //NOTICE 注意这里的最大行号指的是计算的时候的最大行号，此行的和圆的角点在45°圆心角的一边上，之所以这样选择//是因为圆周上的对称特性//这里的二分之根2就是对应那个45°圆心角int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);        //半径的平方const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;//利用圆的方程计算每行像素的u坐标边界（max）for (v = 0; v <= vmax; ++v)umax[v] = cvRound(sqrt(hp2 - v * v));     //结果都是大于0的结果，表示x坐标在这一行的边界//这里其实是使用了对称的方式计算上四分之一的圆周上的umax，目的也是为了保持严格的对称（如果按照常规的想法做，由于cvRound就会很容易出现不对称的情况，//同时这些随机采样的特征点集也不能够满足旋转之后的采样不变性了）for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v){while (umax[v0] == umax[v0 + 1])++v0;umax[v] = v0;++v0;}//!灰度质心法圆形建立完成

关于ORB理论部分参考ORB理解

到这里Tracking.cc中的ORBextractor的构造函数结束，返回Tracking类的构造函数，后续内容是针对双目和RGB-D相机的一些操作，这里先跳过，结束Tracking类的构造函数，返回System.cc中继续执行下一线程。

*************************************************

⑧初始化线程LocalMapping 线程

⑨初始化线程Loop线程

⑩初始化线程View线程

⑪不同的线程之间设置指针

TODO：第八步到第十一步的内容先跳过。

************************************************

此时System.cc的构造函数也结束了，返回到mono_kitti.cc.完成了实例化一个叫SLAM的System对象。

step3 读取图片 + 处理

main()函数继续执行一个for()循环

for (int ni = 0; ni < nImages; ni++)
{……}

这个循环中读取每张图片对应的时间戳，之后SLAM对象的成员函数TrackMonocular将图片和时间戳传给Tracking线程

//图片
im = cv::imread(vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
//时间戳
double tframe = vTimestamps[ni];
SLAM.TrackMonocular(im,tframe);

TrackMonocular函数是System类的成员函数，再次进入到System.cc

这个函数先是对传感器进行检测，然后是检查是否更换模式、检查是否需要复位，之后进入位姿估计环节

//获取相机位姿的估计结果
//!通过输入图片和时间戳获取位姿结果
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

类指针mpTracker使用GrabImageMonocular函数，这个函数输入左目或者RGBA图像，输出世界坐标系到该帧相机坐标系的变换矩阵

继续看它是如何实现的，主要分为三部分：

第一步 将彩色图像转为灰度图像 cvtColor

第二步 构造Frame , 通过一个if 语句实现，如果为真，该帧没用初始化，需要提取两倍的特征点进行初始化；反之，提取指定的特征点。不管真假，都实例化了一个类对象mCurrentFrame，需要看Frame的构造函数。

它的构造函数分为五部分（step1-step5）

step1: nNextId是静态成员变量

// Step 1 帧的ID 自增
mnId=nNextId++;

step2：计算图像金字塔的参数，通过mpORBextractorLeft类指针获取的，前面构建图像金字塔的时候已经计算过

step3：对这个单目图像进行提取特征点，

提取图像的ORB特征点，提取的关键点存放在mvKeys，描述子存放在mDescriptors

ExtractORB(0,imGray);

通过这个函数实现ORB特征点的提取 （重点），它的实现也在Frame.cc

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

*mpORBextractorLeft是ORBextractor的类指针，这里对括号运算符进行了重载

先看这个仿函数是如何实现的，

void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,OutputArray _descriptors)
{
// Step 1 检查图像有效性。如果图像为空，那么就直接返回if(_image.empty())return;//获取图像的大小Mat image = _image.getMat();//判断图像的格式是否正确，要求是单通道灰度值assert(image.type() == CV_8UC1 );// Pre-compute the scale pyramid// Step 2 构建图像金字塔ComputePyramid(image);

这里调用了ComputePyramid函数来构建图像金字塔，也是类ORBextractor的一个成员函数，

为什么要对图片扩展：使用FAST提取特征点时，以像素点为圆心，取半径为3的圆，但是对于边界线上的点无法得到一个完整的圆，因此需要对原始图像扩展三个像素的边界。其次图像要进行高斯滤波（消除噪声），扩展了EDGE_THRESHOLD的边界

/*** 构建图像金字塔* @param image 输入原图像，这个输入图像所有像素都是有效的，也就是说都是可以在其上提取出FAST角点的*/
void ORBextractor::ComputePyramid(cv::Mat image)
{//开始遍历所有的图层for (int level = 0; level < nlevels; ++level){//获取本层图像的缩放系数// 1, 0.833333, 0.694444, 0.578704, 0.482253, 0.401878, 0.334898, 0.279082float scale = mvInvScaleFactor[level];//计算本层图像的像素尺寸大小// [640 x 480], [533 x 400], [444 x 333], [370 x 278], [309 x 231], [257 x 193], [214 x 161], [179 x 134]Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));//全尺寸图像。包括无效图像区域的大小。将图像进行“补边”，EDGE_THRESHOLD区域外的图像不进行FAST角点检测//[678 x 518], [571 x 438], [482 x 371], [408 x 316], [347 x 269], [295 x 231], [252 x 199], [217 x 172]Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);// 定义了两个变量：temp是扩展了边界的图像，masktemp并未使用Mat temp(wholeSize, image.type()), masktemp;// mvImagePyramid 刚开始时是个空的vector<Mat>// 把图像金字塔该图层的图像指针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;}}

sz 存储了本层图像的大小，就是原始图像在不同层的大小

wholeSize存储了一个扩充后的图像大小

temp是扩展了边界的图像

resize将原始图像Image安装sz的大小以线性压缩的形式压缩到mvImagePyramid[level]中

之后需要使用copyMakerBorder函数填充压缩后图像周围扩充区域的像素

到这里图像金字塔建立完成，保存到mvImagePyramid[level]中，继续看这个仿函数代码。

// Step 3 计算图像的特征点，并且将特征点进行均匀化。均匀的特征点可以提高位姿计算精度
// 存储所有的特征点，注意此处为二维的vector，第一维存储的是金字塔的层数，第二维存储的是那一层金字塔图像里提取的所有特征点
vector < vector<KeyPoint> > allKeypoints;
//使用四叉树的方式计算每层图像的特征点并进行分配
ComputeKeyPointsOctTree(allKeypoints);

计算图像的特征点，并且将特征点进行均匀化。

先计算特征点（TODO这里关于网格的概念不是很理解）

注意：图像边界是 (minBorderX,minBorderY) 和 (maxBorderX,maxBorderY)两个点围成的矩形区域，也就是扩展了三个像素的图像大小。

这里对四周扩充了三个像素的图像划分30*30的网格，然后在网格上提取特征点.

width和height扩充三个像素图片的宽度和高度

nCols 就是扩充三个像素后图像每行有多少网格，nRows同理

wCell是每行网格占了多少像素，wCell同理，前面定义是每个网格占30像素，但图片宽高不一定是30倍数。

void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)  //所有的特征点，这里第一层vector存储的是某图层里面的所有特征点，//第二层存储的是整个图像金字塔中的所有图层里面的所有特征点
{//重新调整图像层数allKeypoints.resize(nlevels);//图像cell的尺寸，是个正方形，可以理解为边长in像素坐标const float W = 30;// 对每一层图像做处理//遍历所有图像for (int level = 0; level < nlevels; ++level){//计算这层图像的坐标边界， NOTICE 注意这里是坐标边界，EDGE_THRESHOLD指的应该是可以提取特征点的有效图像边界，后面会一直使用“有效图像边界“这个自创名词const int minBorderX = EDGE_THRESHOLD-3;          //这里的3是因为在计算FAST特征点的时候，需要建立一个半径为3的圆const int minBorderY = minBorderX;                   //minY的计算就可以直接拷贝上面的计算结果了const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;
//存储需要进行平均分配的特征点vector<cv::KeyPoint> vToDistributeKeys;//一般地都是过量采集，所以这里预分配的空间大小是nfeatures*10vToDistributeKeys.reserve(nfeatures*10);//计算进行特征点提取的图像区域尺寸   这里的widt和height是上下左右各扩充了三个像素的大小const float width = (maxBorderX-minBorderX);const float height = (maxBorderY-minBorderY);//nCols 就是扩充三个像素后图像每行有多少网格，nRows同理const int nCols = width/W;const int nRows = height/W;//计算每个图像网格所占的像素行数和列数   ，网格行列一定是大于等于30的const int wCell = ceil(width/nCols);   //ceil向上取整const int hCell = ceil(height/nRows);

通过上面计算知道了扩充三个像素的图像由多少个网格

然后遍历网格：（先行后列）

//开始遍历图像网格，还是以 行 开始遍历的for(int i=0; i<nRows; i++){//计算当前网格初始行坐标const float iniY =minBorderY+i*hCell;//计算当前网格最大的行坐标，这里的+6=+3+3，即考虑到了多出来3是为了cell边界像素进行FAST特征点提取用//前面的EDGE_THRESHOLD指的应该是提取后的特征点所在的边界，所以minBorderY是考虑了计算半径时候的图像边界float maxY = iniY+hCell+6;//如果初始的行坐标就已经超过了有效的图像边界了，这里的“有效图像”是指原始的、可以提取FAST特征点的图像区域if(iniY>=maxBorderY-3)//那么就跳过这一行continue;//如果图像的大小导致不能够正好划分出来整齐的图像网格，那么就要委屈最后一行了if(maxY>maxBorderY)maxY = maxBorderY;//开始 列  的遍历for(int j=0; j<nCols; j++){//计算初始的列坐标const float iniX =minBorderX+j*wCell;//计算这列网格的最大列坐标，+6的含义和前面相同float maxX = iniX+wCell+6;//判断坐标是否在图像中//如果初始的列坐标就已经超过了有效的图像边界了，这里的“有效图像”是指原始的、可以提取FAST特征点的图像区域。//并且应该同前面行坐标的边界对应，都为-3//!BUG  正确应该是maxBorderX-3if(iniX>=maxBorderX-6)continue;//如果最大坐标越界那么委屈一下if(maxX>maxBorderX)maxX = maxBorderX;// FAST提取兴趣点, 自适应阈值//这个向量存储这个cell中的特征点vector<cv::KeyPoint> vKeysCell;//调用opencv的库函数来检测FAST角点FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),  //待检测的图像， 某层金字塔扩充了三个像素后图片的大小vKeysCell,           //存储角点位置的容器iniThFAST,           //检测阈值true);                //使能非极大值抑制//如果这个图像块中使用默认的FAST检测阈值没有能够检测到角点if(vKeysCell.empty()){//那么就使用更低的阈值来进行重新检测FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),  //待检测的图像vKeysCell,      //存储角点位置的容器minThFAST,       //更低的检测阈值true);         //使能非极大值抑制}//当图像cell中检测到FAST角点的时候执行下面的语句if(!vKeysCell.empty()){//遍历其中的所有FAST角点for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++){//NOTICE 到目前为止，这些角点的坐标都是基于图像cell的，现在我们要先将其恢复到当前的【坐标边界】下的坐标//这样做是因为在下面使用八叉树法整理特征点的时候将会使用得到这个坐标//在后面将会被继续转换成为在当前图层的扩充图像坐标系下的坐标//!vKeysCell提取出来的特征点坐标并不是原始图像的坐标(*vit).pt.x+=j*wCell;(*vit).pt.y+=i*hCell;//然后将其加入到”等待被分配“的特征点容器中vToDistributeKeys.push_back(*vit);}//遍历图像cell中的所有的提取出来的FAST角点，并且恢复其在整个金字塔当前层图像下的坐标}//当图像cell中检测到FAST角点的时候执行下面的语句}//开始遍历图像cell的列}//开始遍历图像cell的行

先进行行遍历（Y轴），计算网格的初始坐标iniY，然后计算最大的坐标maxY，之后对行也需要进行同样的操作。

上面代码中使用opencv提供的FAST函数：

//调用opencv的库函数来检测FAST角点
FAST(mvImagePyramid[level].rowRange(iniY, maxY).colRange(iniX, maxX), //待检测的图像，这里就是当前遍历到的图像块vKeysCell,                                                       //存储角点位置的容器iniThFAST,                                                       //一般情况下检测阈值true);                                                           //使能非极大值抑制

将提取到的结果放到容器vKeyCell中，如果没用提取到特征点，会改变阈值（minThFAST）的大小再次使用该函数。

之后需要对vKeyCell中的特征点进行坐标调整，因为这个容器中每个坐标都是以取到的那个网格的左上角为原点，调整坐标后放到容器vToDistributekeys

假如第二个红色网格中有一关键点（x,y），该关键点是以第二个网格坐上角为原点的，需要将坐标变换到以第一个网格左上角为原点

此时 i=0,j=1 变换后的坐标是 x=x+j*wCell=x+wCell , y = y+i*hCwell=y

如果理解错了，请大家评论区留言下

//当图像cell中检测到FAST角点的时候执行下面的语句
if(!vKeysCell.empty())
{//遍历其中的所有FAST角点for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++){//NOTICE 到目前为止，这些角点的坐标都是基于图像cell的，现在我们要先将其恢复到当前的【坐标边界】下的坐标//这样做是因为在下面使用八叉树法整理特征点的时候将会使用得到这个坐标//在后面将会被继续转换成为在当前图层的扩充图像坐标系下的坐标//!vKeysCell提取出来的特征点坐标并不是原始图像的坐标(*vit).pt.x+=j*wCell;(*vit).pt.y+=i*hCell;//然后将其加入到”等待被分配“的特征点容器中vToDistributeKeys.push_back(*vit);}//遍历图像cell中的所有的提取出来的FAST角点，并且恢复其在整个金字塔当前层图像下的坐标
}//当图像cell中检测到FAST角点的时候执行下面的语句

然后进行四叉树筛选特征点，调用DistributeOctTree

可以先去看看四叉树分配特征点的原理

使用四叉树法对一个图像金字塔图层中的特征点进行平均和分发
keypoints = DistributeOctTree(vToDistributeKeys,      //当前图层提取出来的特征点，也即是等待剔除的特征点//NOTICE 注意此时特征点所使用的坐标都是在“半径扩充图像”下的minBorderX, maxBorderX, //当前图层图像的边界，而这里的坐标却都是在“边缘扩充图像”下的minBorderY, maxBorderY,mnFeaturesPerLevel[level], //希望保留下来的当前层图像的特征点个数level);                    //当前层图像所在的图层

节点分配顺序：后加的先分配。比如先分配下图在第19个节点

到这里特征点的提取与分配就完成了，后续是对特征点计算方向，调用computeOrientation

 // compute orientations
//然后计算这些特征点的方向信息，注意这里还是分层计算的
for (int level = 0; level < nlevels; ++level)computeOrientation(mvImagePyramid[level],    //对应的图层的图像allKeypoints[level],  //这个图层中提取并保留下来的特征点容器umax);                  //以及PATCH的横坐标边界

为每个特征点计算了角度，使其具有旋转不变性。

继续ORBextractor.cc看仿函数代码

进行描述子的计算，

// Step 4 拷贝图像描述子到新的矩阵descriptorsMat descriptors;//统计整个图像金字塔中的特征点int nkeypoints = 0;//开始遍历每层图像金字塔，并且累加每层的特征点个数for (int level = 0; level < nlevels; ++level)nkeypoints += (int)allKeypoints[level].size();//如果本图像金字塔中没有任何的特征点if( nkeypoints == 0 )//通过调用cv::mat类的.realse方法，强制清空矩阵的引用计数，这样就可以强制释放矩阵的数据了//参考[https://blog.csdn.net/giantchen547792075/article/details/9107877]_descriptors.release();else{//如果图像金字塔中有特征点，那么就创建这个存储描述子的矩阵，注意这个矩阵是存储整个图像金字塔中特征点的描述子的_descriptors.create(nkeypoints,     //矩阵的行数，对应为特征点的总个数32,            //矩阵的列数，对应为使用32*8=256位描述子CV_8U);            //矩阵元素的格式//获取这个描述子的矩阵信息// ?为什么不是直接在参数_descriptors上对矩阵内容进行修改，而是重新新建了一个变量，复制矩阵后，在这个新建变量的基础上进行修改？descriptors = _descriptors.getMat();}//清空用作返回特征点提取结果的vector容器_keypoints.clear();//并预分配正确大小的空间_keypoints.reserve(nkeypoints);//因为遍历是一层一层进行的，但是描述子那个矩阵是存储整个图像金字塔中特征点的描述子，所以在这里设置了Offset变量来保存“寻址”时的偏移量，//辅助进行在总描述子mat中的定位int offset = 0;//开始遍历每一层图像for (int level = 0; level < nlevels; ++level){//获取在allKeypoints中当前层特征点容器的句柄vector<KeyPoint>& keypoints = allKeypoints[level];//本层的特征点数int nkeypointsLevel = (int)keypoints.size();//如果特征点数目为0，跳出本次循环，继续下一层金字塔if(nkeypointsLevel==0)continue;

然后进行高斯滤波，并且计算高斯滤波后的描述子。

使用GaussianBlur函数消除图像噪声，防止锐利的像素影响描述子的计算。

//  Step 5 对图像进行高斯模糊// 深拷贝当前金字塔所在层级的图像Mat workingMat = mvImagePyramid[level].clone();// 注意：提取特征点的时候，使用的是清晰的原图像；这里计算描述子的时候，为了避免图像噪声的影响，使用了高斯模糊GaussianBlur(workingMat,        //源图像workingMat,        //输出图像Size(7, 7),       //高斯滤波器kernel大小，必须为正的奇数2,                //高斯滤波在x方向的标准差2,                //高斯滤波在y方向的标准差BORDER_REFLECT_101);//边缘拓展点插值类型// Compute the descriptors 计算描述子// desc存储当前图层的描述子Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);// Step 6 计算高斯模糊后图像的描述子computeDescriptors(workingMat,     //高斯模糊之后的图层图像keypoints,     //当前图层中的特征点集合desc,      //存储计算之后的描述子pattern);   //随机采样模板// 更新偏移量的值 offset += nkeypointsLevel;// Scale keypoint coordinates// Step 6 对非第0层图像中的特征点的坐标恢复到第0层图像（原图像）的坐标系下// ? 得到所有层特征点在第0层里的坐标放到_keypoints里面// 对于第0层的图像特征点，他们的坐标就不需要再进行恢复了if (level != 0){// 获取当前图层上的缩放系数float scale = mvScaleFactor[level];// 遍历本层所有的特征点for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)// 特征点本身直接乘缩放倍数就可以了keypoint->pt *= scale;}// And add the keypoints to the output// 将keypoints中内容插入到_keypoints 的末尾// keypoint其实是对allkeypoints中每层图像中特征点的引用，这样allkeypoints中的所有特征点在这里被转存到输出的_keypoints_keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());}

使用computeDescriptors函数计算描述子，desc是计算的结果

computeDescriptors(workingMat,   //高斯模糊之后的图层图像keypoints,     //当前图层中的特征点集合desc,      //存储计算之后的描述子pattern);   //随机采样模板

总结：到这里仿函数代码就结束了，通过重载括号运算符计算了每层图像的特征点，并把它们均匀分步，最后计算了对应的描述子。

Frame.cc 中的 ExtractORB(0,imGray) 结束。

step4：用OpenCV的矫正函数、内参对提取到的特征点进行矫正

UndistortKeyPoints();// Set no stereo information
// 由于单目相机无法直接获得立体信息，所以这里要给右图像对应点和深度赋值-1表示没有相关信息
mvuRight = vector<float>(N,-1);
mvDepth = vector<float>(N,-1);// 初始化本帧的地图点
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
// 记录地图点是否为外点，初始化均为外点false
mvbOutlier = vector<bool>(N,false);

重点看UndistortKeyPoints这个函数，函数中调用opencv的undistortPoints函数

cv::undistortPoints( mat,                //输入的特征点坐标mat,              //输出的校正后的特征点坐标覆盖原矩阵mK,                  //相机的内参数矩阵mDistCoef,            //相机畸变参数矩阵cv::Mat(),            //一个空矩阵，对应为函数原型中的RmK);               //新内参数矩阵，对应为函数原型中的P

step5：计算去畸变后图像边界，将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行。

Tracking.cc中的Frame构造函数执行结束。

第三步跟踪这个函数的返回的结果是 世界坐标系到该帧相机坐标系的变换矩阵

Track();
//返回当前帧的位姿
return mCurrentFrame.mTcw.clone()

*********************

TODO :关于Track函数的具体实现先跳过

追踪线程是ORB-SLAM2的三个线程之一，下一篇文章中会重点介绍。这里先了解下这个线程实现的大概流程：

1.地图初始化，单目初始化和双目、RGB-D相机不同

2.初始化完成后进入相机跟踪，主要由三种跟踪方式

3.相机跟踪成功后对局部地图进行跟踪，这里需要特征匹配得到更多地图点，也要优化相机位姿

4.更新显示线程中的图像、特征点、地图点等信息

5.跟踪成功，更新恒速运动模型

6.清除观测不到的地图点

7.清除恒速模型跟踪中 UpdateLastFrame中为当前帧临时添加的MapPoints（仅双目和rgbd）

8..对当前帧进行判断是否可以作为关键帧

9.删除BA优化中的外点

10.如果初始化和重定位都失败，需要reset

*******************

到这里 mpTracker->GrabImageMonocular(im,timestamp) 这个函数执行完成，之后会返回计算的位姿结果Tcw。

mono_kitti.cc中的SLAM.TrackMonocular(im,tframe)结束。

step4 Shutdown 关闭SLAM系统

主要是对局部地图和回环线程发生终止请求

SLAM.Shutdown();

step5 计算跟踪图片时间的中位数和平均值

cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
cout << "mean tracking time: " << totaltime/nImages << endl;

step6 保存相机轨迹 KeyFrameTrajectory.txt

SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

参考：(1条消息) ORB-SLAM2：Tracking线程学习随笔【李哈哈：看看总有收获篇】_正在刷夜的李哈哈的博客-CSDN博客https://blog.csdn.net/qq_41883714/article/details/114655311?spm=1001.2014.3001.5502(1条消息) 一步步带你看懂orbslam2源码--总体框架(一)_Mr.Silver的博客-CSDN博客_orbslam2框架https://blog.csdn.net/qq_37708045/article/details/101751343

ORB-SLAM2源码学习（一）相关推荐

ORB SLAM2源码解读(三)：Frame类
文章目录前言构造函数双目相机 RGBD相机单目相机 ExtractORB:提取特征点 ComputeBoW:计算词袋数据 SetPose:设置相机外参 isInFrustum:判断一个MapP ...
Shiro源码学习之二
接上一篇 Shiro源码学习之一 3.subject.login 进入login public void login(AuthenticationToken token) throws Authent ...
Shiro源码学习之一
一.最基本的使用 1.Maven依赖 <dependency><groupId>org.apache.shiro</groupId><artifactId&g ...
mutations vuex 调用_Vuex源码学习（六）action和mutation如何被调用的（前置准备篇）...
前言 Vuex源码系列不知不觉已经到了第六篇.前置的五篇分别如下: 长篇连载:Vuex源码学习(一)功能梳理长篇连载:Vuex源码学习(二)脉络梳理作为一个Web前端,你知道Vuex的instal ...
vue实例没有挂载到html上,vue 源码学习 - 实例挂载
前言在学习vue源码之前需要先了解源码目录设计(了解各个模块的功能)丶Flow语法. src ├── compiler # 把模板解析成 ast 语法树,ast 语法树优化,代码生成等功能. ├── ...
2021-03-19Tomcat源码学习--WebAppClassLoader类加载机制
Tomcat源码学习--WebAppClassLoader类加载机制在WebappClassLoaderBase中重写了ClassLoader的loadClass方法,在这个实现方法中我们可以一窥t ...
jQuery源码学习之Callbacks
jQuery源码学习之Callbacks jQuery的ajax.deferred通过回调实现异步,其实现核心是Callbacks. 使用方法使用首先要先新建一个实例对象.创建时可以传入参数flag ...
JDK源码学习笔记——Integer
一.类定义 public final class Integer extends Number implements Comparable<Integer> 二.属性 private fi ...
DotText源码学习——ASP.NET的工作机制
--本文是<项目驱动学习--DotText源码学习>系列的第一篇文章,在这之后会持续发表相关的文章. 概论在阅读DotText源码之前,让我们首先了解一下ASP.NET的工作机制,可以使 ...
Vuex源码学习（五）加工后的module
没有看过moduleCollection那可不行!Vuex源码学习(四)module与moduleCollection 感谢提出代码块和截图建议的小伙伴代码块和截图的区别: 代码块部分希望大家按照我 ...

ORB-SLAM2源码学习（一）

ORB-SLAM2源码学习（一）相关推荐

最新文章

热门文章