ORB2单目读代码笔记3--ORB特征提取构造函数仿函数、图像帧构造函数、计算图像金字塔并进行扩充

3.ORB特征提取构造函数仿函数、图像帧构造函数、计算图像金字塔并进行扩充

- Tracking结束,返回System
- System未结束,暂时返回main
- main 跳转 TrackMonocular
- TrackMonocular 跳转 GrabImageMonocular
- GrabImageMonocular 跳转 Frame
- Frame 跳转 ExtractORB
- ExtractORB 跳转 operator()
- operator() 跳转 ComputePyramid
- ComputePyramid 结束,返回 operator()

输出提取器的信息:

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;
        else
            mDepthMapFactor = 1.0f/mDepthMapFactor;

Tracking结束,返回System

后面的建图线程,回环检测线程暂时跳过,先返回main

System未结束,暂时返回main

nImages

是

int

类型,存储的是rgb图片名字长度

vTimesTrack

是

vector

类型,存储跟踪的时间统计数据

// Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

im

是

Mat

类型,读取对应数据路径下rgb图片

tframe

是

double

类型,存储rgb图片对应时间戳

// Main loop
    cv::Mat im;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file
        im = cv::imread(string(argv[3])+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];

判断是否读到了图片

if(im.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << string(argv[3]) << "/" << vstrImageFilenames[ni] << endl;
            return 1;
        }

宏定义统计时间

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

单目跟踪线程启动

// Pass the image to the SLAM system
        SLAM.TrackMonocular(im,tframe);

main 跳转 TrackMonocular

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
{

判断传感器类型是否符合

if(mSensor!=MONOCULAR)
    {
        cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;
        exit(-1);
    }

根据

mbActivateLocalizationMode

判断是否关闭局部地图线程

如果关闭,则只计算位姿,不更新局部地图

// Check mode change
    {
        // 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱
        unique_lock<mutex> lock(mMutexMode);
        // mbActivateLocalizationMode为true会关闭局部地图线程
        if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped())
            {
                usleep(1000);
            }

            // 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新
            // 设置mbOnlyTracking为真
            mpTracker->InformOnlyTracking(true);
            // 关闭线程可以使得别的线程得到更多的资源
            mbActivateLocalizationMode = false;

根据

mbDeactivateLocalizationMode

判断是否释放局部地图线程

// 如果mbDeactivateLocalizationMode是true，局部地图线程就被释放, 关键帧从局部地图中删除.
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            mbDeactivateLocalizationMode = false;
        }
    }

检查复位

// Check reset
    {
    unique_lock<mutex> lock(mMutexReset);
    if(mbReset)
    {
        mpTracker->Reset();
        mbReset = false;
    }
    }

使用

mpTracker->GrabImageMonocular

计算单目相机位姿

并用

Tcw

存储位姿

//获取相机位姿的估计结果
    cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

TrackMonocular 跳转 GrabImageMonocular

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im,const double &timestamp)
{
    mImGray = im;

将输入的rgb图像转换为灰度图

// Step 1 ：将彩色图像转为灰度图像
    //若图片是3、4通道的，还需要转化成灰度图
    if(mImGray.channels()==3)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGB2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGR2GRAY);
    }
    else if(mImGray.channels()==4)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
    }

构造帧Frame,如果之前没有初始化过帧,则执行下面代码,有没有初始化的区别是,ORB特征提取器使用的不同,未初始化的帧使用

mpIniORBextractor

,会提取2倍特征点:

// Step 2 ：构造Frame
    //判断该帧是不是初始化
    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET) //没有成功初始化的前一个状态就是NO_IMAGES_YET
        mCurrentFrame = Frame(
            mImGray,
            timestamp,
            mpIniORBextractor,      //初始化ORB特征点提取器会提取2倍的指定特征点数目
            mpORBVocabulary,
            mK,
            mDistCoef,
            mbf,
            mThDepth);

初始化的帧使用的是

mpORBextractorLeft

,正常提取指定数目特征点:

else
        mCurrentFrame = Frame(
            mImGray,
            timestamp,
            mpORBextractorLeft,     //正常运行的时的ORB特征点提取器，提取指定数目特征点
            mpORBVocabulary,
            mK,
            mDistCoef,
            mbf,
            mThDepth);

GrabImageMonocular 跳转 Frame

Frame构造函数:

Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, 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)
{

帧ID自增1,其中

nNextId

是static类的静态成员变量,在全局区初始化

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

读取图像金字塔参数

// Step 2 计算图像金字塔的参数 
    //获取图像金字塔的层数
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    //获取每层的缩放因子
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();    
    //计算每层缩放因子的自然对数
    mfLogScaleFactor = log(mfScaleFactor);
    //获取各层图像的缩放因子
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    //获取各层图像的缩放因子的倒数
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    //TODO 也是获取这个不知道有什么实际含义的sigma^2
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    //计算上面获取的sigma^2的倒数
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

提取ORB特征点,第一个参数0表示左图,1表示右图

/** 3\. 提取彩色图像(其实现在已经灰度化成为灰度图像了)的特征点 \n Frame::ExtractORB() */

    // ORB extraction
    // Step 3 对图像进行提取特征点, 第一个参数0-左图， 1-右图
    ExtractORB(0,imGray);

Frame 跳转 ExtractORB

根据左图右图选择不同的提取特征点函数

这里

(*mpORBextractorLeft)

和

(*mpORBextractorRight)

是用了ExtractORB类中重载的括号

( )

是仿函数

3.4为什么要重載小括号运算符 operator0

可以用于仿函数（一个可以实现函数功能的对象）

仿函数( functor)又称为函数对象( function object)是一个能行使函数功能的类。仿函数的语法几乎和我们普通的函数调用一样，不过作为仿函数的类，都必须重载 operator()运算符

1.仿函数可有拥有自己的数据成员和成员变量，这意味着这意味着仿函数拥有状态。这在一般函数中是不可能的。

2.仿函数通常比一般函数有更好的速度

类似于函数指针??

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

ExtractORB 跳转 operator()

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

构建图像金字塔

ComputePyramid

// Pre-compute the scale pyramid
    // Step 2 构建图像金字塔
    ComputePyramid(image);

operator() 跳转 ComputePyramid

void ORBextractor::ComputePyramid(cv::Mat image)
{

计算每个图层的信息

//开始遍历所有的图层
    for (int level = 0; level < nlevels; ++level)
    {

每个图层缩放系数和图像大小信息

其中

sz

指的是源图像大小,而

wholeSize

指的是扩充后图像大小:

ORB2单目读代码笔记3--ORB特征提取构造函数仿函数、图像帧构造函数、计算图像金字塔并进行扩充

//获取本层图像的缩放系数
        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

存储扩展图像,用Rect把原图像抠出来赋值给

temp

,于是

temp

就变成了大小为扩展图像,内部有一个大小为原图像的区域的Mat对象.

然后将

temp

传给当前金字塔层.

其中,

Rect

是cv矩阵类,四个参数分别是x坐标,y坐标,宽,高:

ORB2单目读代码笔记3--ORB特征提取构造函数仿函数、图像帧构造函数、计算图像金字塔并进行扩充

// 定义了两个变量：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));

如果当前层数不为0,将原图像按一定缩放系数处理后拷贝到当前金字塔层中间的矩形区域

然后把当前层的图像拷贝给

temp

copyMakeBorder

作用是将第一个参数拷贝到第二个参数,边界留白长度为第三四五六个参数

// Compute the resized image
        //计算第0层以上resize后的图像
        if( level != 0 )
        {
            //!  原代码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

如果当前层数为0,将原图像拷贝到

temp

中间矩形部分

copyMakeBorder

作用是将第一个参数拷贝到第二个参数,边界留白长度为第三四五六个参数

else
        {
            //对于第0层未缩放图像，直接将图像深拷贝到temp的中间，并且对其周围进行边界扩展。此时temp就是对原图扩展后的图像
            copyMakeBorder(image,           //这里是原图像
                           temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
                           BORDER_REFLECT_101);            
        }

最后将

temp

拷贝给当前金字塔层,当前层金字塔构建完毕,继续进入下一层

//! 原代码mvImagePyramid 并未扩充，应该添加下面一行代码
        mvImagePyramid[level] = temp;
    }

}

ORB2单目读代码笔记3--ORB特征提取构造函数仿函数、图像帧构造函数、计算图像金字塔并进行扩充

3.ORB特征提取构造函数仿函数、图像帧构造函数、计算图像金字塔并进行扩充

Tracking结束,返回System

System未结束,暂时返回main

main 跳转 TrackMonocular

TrackMonocular 跳转 GrabImageMonocular

GrabImageMonocular 跳转 Frame

Frame 跳转 ExtractORB

ExtractORB 跳转 operator()

operator() 跳转 ComputePyramid

ComputePyramid 结束,返回 operator()

继续阅读

单目初始化（七）：使用基础矩阵恢复位姿

三角测量及在SLAM中的使用

从零开始跑ORB_SLAM2(三) 使用Realsense D435相机跑ORB_SLAM2生成稀疏点阵

ORBSLAM2理论与实战（5）运行实战

ubuntu16.04编译ORBSLAM2问题解决

ORBSLAM2代码2-图像金字塔一.前言二.代码

一步步带你看懂orbslam2源码--单应矩阵/基础矩阵,求解R,t(四)

ORB-SLAM2-Brief描述子方向计算代码

kitti2bag工具的安装

ORBSLAM2学习（六）：DLoopDetector理论知识学习0 前言1 工程下载与编译2 算法原理

ORBSLAM2理论与实战（16）代码解读Map类

ORBSLAM2理论与实战（14）代码解读KeyFrame类

ORBSLAM2理论与实战（17）代码解读MapPoint类

ORBSLAM2理论与实战（3） SLAM常用术语

orbslam2 ROS下RGBD实验记录(三) KINECT2通过ROS在线跑ORBSLAM2

30局部建图线程中的localBAORBSLAM2代码