ORBSLAM2单目读代码笔记2--SLAM系统构造函数、特征点图像金字塔、构造灰度质心圆

最新推荐文章于 2022-12-04 11:54:38 发布

不能再吃了OvO

最新推荐文章于 2022-12-04 11:54:38 发布

阅读量578

点赞数 5

分类专栏： ORBSLAM2

本文链接：https://blog.csdn.net/klsjadkls/article/details/115604469

版权

ORBSLAM2 专栏收录该内容

14 篇文章 14 订阅

订阅专栏

2.SLAM系统构造函数、特征点图像金字塔、构造灰度质心圆

mono_tum.cc

mono_tum.cc

int main(int argc, char **argv)

检查参数是否为4.参数分别为

可执行文件
ORB字典文件路径
配置文件路径
数据集路径

if(argc != 4)
    {
        cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" << endl;
        return 1;
    }

下面的代码用于读取rgb数据,

其中strFile是rgb.txt的路径
vTimestamps用于存储rgb图片时间戳
vstrImageFilenames用于存储rgb图片名字
函数LoadImages用于把rgb.txt中的图片名字和时间戳数据分别存放到前两个容器中

   // Retrieve paths to images
    vector<string> vstrImageFilenames;
    vector<double> vTimestamps;
    string strFile = string(argv[3])+"/rgb.txt";
    LoadImages(strFile, vstrImageFilenames, vTimestamps);

main 跳转 LoadImages

其中,LoadImages函数实现如下:

void LoadImages(const string &strFile, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
    ifstream f;
    f.open(strFile.c_str());

    // skip first three lines
    // rgb.txt前三行是注释，存储到s0中,不使用
    string s0;
    getline(f,s0);
    getline(f,s0);
    getline(f,s0);

    //若f文件流不为空,则执行循环
    while(!f.eof())
    {
        //创建string类型s每次存储rgb.txt的一行数据
        //rgb.txt中第一个数据为时间戳,第二个数据为图片名字
        string s;
        getline(f,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;            //ss读取s全部数据(时间戳+图片名)
            double t;
            string sRGB;
            ss >> t;            //每次输出一个数据
            vTimestamps.push_back(t);
            ss >> sRGB;         //输出第二个数据
            vstrImageFilenames.push_back(sRGB);
        }
    }

LoadImages 结束 , 返回 main

下面是ORBSLAM2的初始化函数:

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

main 跳转 System

先看System的构造函数:

System(const string &strVocFile,            //指定ORB字典文件的路径
           const string &strSettingsFile,       //指定配置文件的路径
           const eSensor sensor,                //指定所使用的传感器类型
           const bool bUseViewer = true);       //指定是否使用可视化界面 TODO

实现如下:

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)        //没有这个模式转换标志
{

输出欢迎信息

    cout << endl <<
    "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<
    "This program comes with ABSOLUTELY NO WARRANTY;" << endl  <<
    "This is free software, and you are welcome to redistribute it" << endl <<
    "under certain conditions. See LICENSE.txt." << endl << endl;

    // 输出当前传感器类型
    cout << "Input sensor was set to: ";

    if(mSensor==MONOCULAR)
        cout << "Monocular" << endl;
    else if(mSensor==STEREO)
        cout << "Stereo" << endl;
    else if(mSensor==RGBD)
        cout << "RGB-D" << endl

检查配置文件

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

加载字典文件,ORBVocabulary()是DBoW库中的函数,不要求实现底层

   cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;

    //建立一个新的ORB字典
    mpVocabulary = new ORBVocabulary();
    //获取字典加载状态
    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;

创建关键帧库

    //Create KeyFrame Database
    mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

创建地图

    //Create the Map
    mpMap = new Map();

创建帧绘制器和地图绘制器

    //Create Drawers. These are used by the Viewer
    //这里的帧绘制器和地图绘制器将会被可视化的Viewer所使用
    mpFrameDrawer = new FrameDrawer(mpMap);
    mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

追踪线程初始化函数

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

System 跳转 Tracking

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,没有进行这个过程的时候的默认值

首先从.yaml中读取数据,FileStorage设定为READ,可以将" "内的字符串对应的数据读出:

    // 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"];

构造相机内参矩阵,其中Mat.at可以读写指定矩阵位置:

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

输出读取到的参数:

    //输出
    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;

设置RBG还是BGR:

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

加载ORB特征点参数:

    // 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作为特征点提取器
    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);

Tracking 跳转 ORBextractor

ORB提取器实现如下:

参数:

ORBextractor::ORBextractor(int _nfeatures,      //指定要提取的特征点数目
                           float _scaleFactor,  //指定图像金字塔的缩放系数
                           int _nlevels,        //指定图像金字塔的层数
                           int _iniThFAST,      //指定初始的FAST特征点提取参数，可以提取出最明显的角点
                           int _minThFAST):     //如果因为图像纹理不丰富提取出的特征点不多，为了达到想要的特征点数目，
                                                //就使用这个参数提取出不是那么明显的角点
    nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
    iniThFAST(_iniThFAST), minThFAST(_minThFAST)//设置这些参数

设置图像金字塔参数:

    //存储每层图像缩放系数的vector调整为符合图层数目的大小
    mvScaleFactor.resize(nlevels);  
    //存储这个sigma^2，其实就是每层图像相对初始图像缩放因子的平方
    mvLevelSigma2.resize(nlevels);
    //对于初始图像，这两个参数都是1
    mvScaleFactor[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);
    //每层需要提取出来的特征点个数，这个向量也要根据图像金字塔设定的层数进行调整
    mnFeaturesPerLevel.resize(nlevels);

补 : 图像金字塔

图像金字塔

金字塔层数越高，图像的面积越小，所能提取到的特征数量就越小。基于这个原理，我们可以按照面积将特征点均到金字塔每层的图像上。我们假设第0层图像的宽为 ${W}$ ,长为 ${L}$ ,缩放因子为 ${S}$ (这里的 ${0<S<1}$ )。那么整个金字塔总的面积为

金字塔总的面积

那么，单位面积的特征点数量为

单位面积的特征点数量

那么，第0层应分配的特征点数量为

第0层应分配的特征点数量

接着那么，推出了第a层应分配的特征点数量为

第a层应分配的特征点数量

实际上， Opencv里的代码不是按面积算的，而是按照边长来算的，也就是上面公式中的 ${s^2}$ 换成 ${s}$ 。

    //图片降采样缩放系数的倒数
    float factor = 1.0f / scaleFactor;
    //每个单位缩放系数所希望的特征点个数
    float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));

这里的 nfeature 相当于式子中的 ${N}$ , ${1 - s^2}$ 相当于 ${1 - factor}$ .

pow是幂,即 factor 的 nlevels 次幂.

然后计算每层金字塔的特征点数,思路是求出 level0 的层数,逐层乘系数,cvRound作用是取整数,多余的特征点数给最高层:

    //用于在特征点个数分配的，特征点的累计计数清空
    int sumFeatures = 0;
    //开始逐层计算要分配的特征点个数，顶层图像除外（看循环后面）
    for( int level = 0; level < nlevels-1; level++ )
    {
        //分配 cvRound : 返回个参数最接近的整数值
        mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);
        //累计
        sumFeatures += mnFeaturesPerLevel[level];
        //乘系数
        nDesiredFeaturesPerScale *= factor;
    }
   //由于前面的特征点个数取整操作，可能会导致剩余一些特征点个数没有被分配，所以这里就将这个余出来的特征点分配到最高的图层中
    mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);

补 : 描述子选点模式

赋值成员变量pattern , pattern是BRIEF描述子选取点对的模式,std::copy的作用是把输入序列开始(第一个参数)到结束(第二个参数)的数据拷贝到容器(第三个参数,使用back_insert_iterator迭代器)中:

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

下面求umax , umax是当取到上面圆里的行坐标(v轴)时,对应的u轴坐标值,目的是当给出一个v轴坐标,能马上求出圆上对应像素的u轴坐标:

    //This is for orientation
    //下面的内容是和特征点的旋转计算有关的
    // pre-compute the end of a row in a circular patch
    //预先计算圆形patch中行的结束位置
    //+1中的1表示那个圆的中间行
    umax.resize(HALF_PATCH_SIZE + 1);

其中vmin是四分之一圆0~45度时的对应的45度时候的v轴坐标

vmax是四分之一圆90~45度时的对应的45度时候的v轴坐标

给v求u

把四分之一圆分成两段对称的八分之一圆来求解的原因是:

代码中使用了cvRound函数,是四舍五入函数,为了保证对称性,求解时,分别从0度和90度对称地逼近45度

    //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坐标在这一行的边界

    // Make sure we are symmetric
    //这里其实是使用了对称的方式计算上四分之一的圆周上的umax，目的也是为了保持严格的对称（如果按照常规的想法做，由于cvRound就会很容易出现不对称的情况，
    //同时这些随机采样的特征点集也不能够满足旋转之后的采样不变性了）
    for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
    {
        while (umax[v0] == umax[v0 + 1])
            ++v0;
        umax[v] = v0;
        ++v0;

ORBextractor结束,返回Tracking

总结:ORBextractor作用是初始化ORB特征提取器,输入参数是

要提取的特征点数目
图像金字塔的缩放系数
图像金字塔的层数
初始的FAST特征点提取参数，可以提取出最明显的角点

不能再吃了OvO

关注

5
点赞
踩
7

收藏

觉得还不错? 一键收藏
2
评论
ORBSLAM2单目读代码笔记2--SLAM系统构造函数、特征点图像金字塔、构造灰度质心圆

2.SLAM系统构造函数、特征点图像金字塔、构造灰度质心圆mono_tum.ccint main(int argc, char **argv)检查参数是否为4.参数分别为可执行文件ORB字典文件路径配置文件路径数据集路径if(argc != 4) { cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" <&lt
复制链接

扫一扫