零基础学习ORB-SLAM2特征点提取-从原理到源码【李哈哈】

写在前边的话

该文章将介绍ORB-SLAM2源代码中特征提取部分的主题内容，涉及整体流程、原理简介、源代码讲解，那我们开始吧！
强烈建议配合B站视频食用，效果更更佳！也是我自己做der：
正在刷夜的李哈哈B站主页

框架梳理

mono_kitti.cc 1.0

我们采用mono_kitti.cc来开始程序，也就是针对单目kitti数据集来研究。
进入main函数，定义了两个容器来存放每张图片的路径和时间戳：

// Retrieve paths to images
vector<string> vstrImageFilenames; //图像序列中每张图片存放路径
vector<double> vTimestamps;        //图像序列中每张图片时间戳

然后执行LoadImages函数：

//!这是一个非常关键的读文件函数
//argv[3]存放的是图像序列的存放路径，是从外界输入的路径
LoadImages(string(argv[3]), vstrImageFilenames, vTimestamps);

该函数也在mono_kitti.cc文件中，函数定义如下：

//以下为LoadImages函数定义，这是该文件夹唯一的函数
// 函数定义：获取图像序列中每一张图像的访问路径和时间戳
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
   
    // step 1 读取时间戳文件
    ifstream fTimes; //定义一个输入流fTimes来读取文件中的时间戳(ifstream用来读取)
    //调用数据集中的时间戳文件
    string strPathTimeFile = strPathToSequence + "/times.txt"; //strPathTimeFile是一个路径，末位追踪到time.txt

    //c_str是string类的一个函数，可以把string类型变量转换成char*变量
    //open()要求的是一个char*字符串
    //当文件名是string时需要转换，当文件名是字符数组型时就不需要此转换
    fTimes.open(strPathTimeFile.c_str()); //使用输入流读取文件，调用了open()函数
    while (!fTimes.eof())                 //.eof()函数判断文件夹是否读到最后
    {
   
        string s;
        getline(fTimes, s); //自动读取下一行时间戳文件
        // 如果该行字符串不是空的，就写进流，读成double类型放入vector（用来改变数据类型）
        if (!s.empty())
        {
   
            //流对象，用于流的输入输出
            stringstream ss;
            ss << s;
            double t;
            ss >> t;
            // 保存时间戳
            vTimestamps.push_back(t);
            //!至此，vTimestamps储存了所有的时间戳
        }
    }

    // step 1 使用左目图像, 生成左目图像序列中的每一张图像的文件名
    //组装mono_kitti数据集中image_0目录的路径
    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，其他用0填充
        vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png";
        //!至此，组装形成包含图像路径和编号的vector
    }
}

执行完该函数后，成功将每张图片的路径及时间戳读取到刚刚定义的两个vector容器中
main函数继续执行，定义了变量nImages来存储有多少个图片：

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

main函数继续执行，实例化一个SLAM对象：

//!实例化SLAM对象
// Create SLAM system. It initializes all system threads and gets ready to process frames.
//argv1是词袋地址，argv2是配置文件的地址，第三个参数是传感器类型，第四个参数为是否选择使用可视化界面
//argv[1]为vocfile 里边存储的是词汇
//argv[2]为settingfile 里边存储摄像机校准和畸变参数和ORB相关参数
//这里创建了System类型的SLAM对象，SLAM构造函数中初始化了系统所有线程和相关参数，并准备好处理帧，代码留待后边详细分析
//读入词包路径，读入YAML配置文件，设置SLAM为mono状态,启用viewer的线程简要说明
ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true);

这里说是实例化SLAM对象，其实是指创建了一个System类的对象，并取名为SLAM，该类的定义在System.cc文件

System.cc 1.0

打开该文件，映入眼帘的是System类的有参构造函数：

namespace ORB_SLAM2
{
   
    //!系统的构造函数，将会启动其他的线程
    System::System(const string &strVocFile,      //词典文件路径
                   const string &strSettingsFile, //配置文件路径
                   const eSensor sensor,          //传感器类型
                   const bool bUseViewer)         //是否使用可视化界面
        :                                         //下方为成员变量默认初始化内容
          mSensor(sensor),                        //初始化传感器类型
          mpViewer(static_cast<Viewer *>(NULL)),  //空。。。对象指针？  TODO
          mbReset(false),                         //无复位标志
          mbActivateLocalizationMode(false),      //没有这个模式转换标志
          mbDeactivateLocalizationMode(false)     //没有这个模式转换标志
    {
   
        //!这里是构造函数的具体内容

可以看到，这里有一个叫做ORB_SLAM2的名称空间，仔细翻看各个源文件，发现都是在开头有个这样的名称空间，可以先认为就是一个大名😜，我还不知道有什么特别的用处？
在该构造函数中，依次完成了如下步骤：
①在终端输出欢迎信息
②在终端输出传感器类型
③读取词袋，存入在System类中定义的ORBVocabulary类的类指针==*mpVocabulary==，该定义在System.h中，读取语句如下：

mpVocabulary = new ORBVocabulary(); //来自第三方库，暂时不需要很详细的了解

④利用词袋初始化关键帧数据库，存入关键帧数据库指针mpKeyFrameDatabase
⑤创建一个Map类，存储指向所有关键帧和数据点的指针mpMap
⑥创建两个窗口，用来绘制帧和地图mpFrameDrawer``mpMapDrawer
⑦初始化Tracking线程
⑧初始化Local Mapping线程
⑨初始化Loop Closing线程
⑩初始化Viewer线程
⑪在不同线程间设置指针，创建资源联系
首先要从Tracking线程讲起，在该构造函数的第⑦步中，相关代码如下：

//在本主进程中初始化追踪线程
        //Initialize the Tracking thread
        //(it will live in the main thread of execution, the one that called this constructor)
        //这里初始化的Tracking线程是在main线程中运行的(其实也就是main线程)，所以不需要调用new thread来创建
        //!这里是重点，Tracking函数
        mpTracker = new Tracking(this,               //现在还不是很明白为什么这里还需要一个this指针  TODO
                                 mpVocabulary,       //字典
                                 mpFrameDrawer,      //帧绘制器
                                 mpMapDrawer,        //地图绘制器
                                 mpMap,              //地图
                                 mpKeyFrameDatabase, //关键帧地图
                                 strSettingsFile,    //设置文件路径
                                 mSensor);           //传感器类型iomanip

这里根据类Tracking的有参构造函数，传入了一系列参数，比如刚刚提到的词袋指针mpVocabulary
进入类Tracking的定义，跳转到定义该类的Tracking.cc文件

Tracking.cc 1.0

这里看似一个函数，其实完成了一系列操作，构造函数代码如下：

//Tracking类的构造函数
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,没有进行这个过程的时候的默认值
{
   
    // Load camera parameters from settings file
    // 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);
    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]
    cv::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 50
    mbf = 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
    int nRGB = fSettings["Camera.RGB"];
    mbRGB = nRGB;

    if (mbRGB)
            cout << "- color order: RGB (ignored if grayscale)" << endl;
    else
            cout << "- color order: BGR (ignored if grayscale)" << endl;

    // Load ORB parameters

    // Step 2 加载ORB特征点有关的参数,并新建特征点提取器

    // 每一帧提取的特征点数 1000
    int nFeatures = fSettings["ORBextractor.nFeatures"];
    // 图像建立金字塔时的变化尺度 1.2
    float fScaleFactor = fSettings["ORBextractor.scaleFactor"];
    // 尺度金字塔的层数 8
    int nLevels = fSettings["ORBextractor.nLevels"];
    // 提取fast特征点的默认阈值 20
    int fIniThFAST = fSettings["ORBextractor.iniThFAST"];
    // 如果默认阈值提取不出足够fast特征点，则使用最小阈值 8
    int fMinThFAST = fSettings["ORBextractor.minThFAST"];

    // !tracking过程都会用到mpORBextractorLeft作为特征点提取器（单目就是left）
    // !开始提取ORB特征！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！
    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) //单目初始化要提取两倍的特征feature
            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;