ORBSLAM2代码2-图像金字塔

---

一.前言

这一部分主要是对输入进来的图像进行构建图像金字塔，并且对每层图像的边缘进行填充，方便后续的特征点提取。图像帧Frame的构造函数还没完成。

二.代码

这部分代码在mono_um.cc中

SLAM.TrackMonocular(im,tframe);

进入TrackMonocular这个函数中，这个函数在system.cc中

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

    // Check mode change//下面这部分是关于模式选择，是否局部建图
    {
        unique_lock<mutex> lock(mMutexMode);
        if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();

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

            mpTracker->InformOnlyTracking(true);
            mbActivateLocalizationMode = false;
        }
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            mbDeactivateLocalizationMode = false;
        }
    }

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

    cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);//最重要的是这个函数，返回位姿

    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;

    return Tcw;
}

进入GrabImageMonocular这个函数，在Tracking.cc这个文件中

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)//传入的参数是图片与时间戳
{
    mImGray = im;

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

    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)//判断是不是没有初始化
        mCurrentFrame = Frame(
        mImGray,//图像
        timestamp,//时间错
        mpIniORBextractor,//ORB提取器
        mpORBVocabulary,//磁带
        mK,//内参
        mDistCoef,//畸变参数
        mbf,//双目基线
        mThDepth);//深度
    else
        mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

    Track();

    return mCurrentFrame.mTcw.clone();
}

进入Frame

Frame::Frame(const cv::Mat &imGray, 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)
{
    // Frame ID
    mnId=nNextId++;//首先给ID+1。这个是个全局静态变量

    // Scale Level Info//构建图像金字塔
    mnScaleLevels = mpORBextractorLeft->GetLevels();//获取层数
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();//获取缩放尺度
    mfLogScaleFactor = log(mfScaleFactor);//尺度对数
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();//获取各层缩放尺度
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();//缩放尺度倒数
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();//平方
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();//平方的倒数

    // ORB extraction
    ExtractORB(0,imGray);//特征点的提取，0表示的是左相机，当然单目里不分

    N = mvKeys.size();

    if(mvKeys.empty())
        return;

    UndistortKeyPoints();

    // Set no stereo information
    mvuRight = vector<float>(N,-1);
    mvDepth = vector<float>(N,-1);

    mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
    mvbOutlier = vector<bool>(N,false);

    // This is done only for the first Frame (or after a change in the calibration)
    if(mbInitialComputations)
    {
        ComputeImageBounds(imGray);

        mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
        mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);

        fx = K.at<float>(0,0);
        fy = K.at<float>(1,1);
        cx = K.at<float>(0,2);
        cy = K.at<float>(1,2);
        invfx = 1.0f/fx;
        invfy = 1.0f/fy;

        mbInitialComputations=false;
    }

    mb = mbf/fx;

    AssignFeaturesToGrid();
}

进入ExtractORB，提取特征，在frame.cc中

void Frame::ExtractORB(int flag, const cv::Mat &im)
{
    if(flag==0)
        (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);//这边这个括号其实是一个仿函数，
    else
        (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}

在这里，这个（）是一个仿函数，也就是说在ORBextractor这个类中对这个（）进行了重载，代码如下：

void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
                      OutputArray _descriptors)//参数是，图像;掩膜？？？;特征点;描述子
{ 
    if(_image.empty())//判断图像是否是空的
        return;

    Mat image = _image.getMat();//获取图像大小
    assert(image.type() == CV_8UC1 );//判断是不是灰度图

    // Pre-compute the scale pyramid
    ComputePyramid(image);//计算图像金字塔，看下面具体函数

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

    Mat descriptors;

    int nkeypoints = 0;
    for (int level = 0; level < nlevels; ++level)
        nkeypoints += (int)allKeypoints[level].size();
    if( nkeypoints == 0 )
        _descriptors.release();
    else
    {
        _descriptors.create(nkeypoints, 32, CV_8U);
        descriptors = _descriptors.getMat();
    }

    _keypoints.clear();
    _keypoints.reserve(nkeypoints);

    int offset = 0;
    for (int level = 0; level < nlevels; ++level)
    {
        vector<KeyPoint>& keypoints = allKeypoints[level];
        int nkeypointsLevel = (int)keypoints.size();

        if(nkeypointsLevel==0)
            continue;

        // preprocess the resized image
        Mat workingMat = mvImagePyramid[level].clone();
        GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);

        // Compute the descriptors
        Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
        computeDescriptors(workingMat, keypoints, desc, pattern);

        offset += nkeypointsLevel;

        // Scale keypoint coordinates
        if (level != 0)
        {
            float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor);
            for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
                 keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
                keypoint->pt *= scale;
        }
        // And add the keypoints to the output
        _keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
    }
}

构建图像金字塔ComputePyramid，在ORBextractor。cpp中

void ORBextractor::ComputePyramid(cv::Mat image)//传入的参数是图像
{
    for (int level = 0; level < nlevels; ++level)//判断层数是不是小于8
    {
        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);//这边在原图上面往外扩充了EDGE_THRESHOLD=19大小，具体作用暂不知道
        Mat temp(wholeSize, image.type()), masktemp;
        mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));//这里面就是图像原图，并没有包含扩充之后的部分，并赋给金字塔相应层数

        // Compute the resized image
        if( level != 0 )
        {//依据上一层，将图像resize到当前层的大小
            resize(mvImagePyramid[level-1],//输入上一层金字塔图
             mvImagePyramid[level],//输出当前金字塔图
              sz,//输出图像尺寸
               0, //水平方向缩放系数，0表示自动计算
               0, //垂直方向缩放系数，0表示自动计算
               INTER_LINEAR);//图像缩放的差值算法类型，这里的是线性插值算法
//把源图像拷贝到目的图像的中央，四面填充指定的像素。图片如果已经拷贝到中间，只填充边界
            copyMakeBorder(mvImagePyramid[level], //源图像
            temp, //目标图像（此时其实就已经有大了一圈的尺寸了）
            EDGE_THRESHOLD, 
            EDGE_THRESHOLD, 
            EDGE_THRESHOLD,
            EDGE_THRESHOLD,
            BORDER_REFLECT_101+BORDER_ISOLATED);     //扩充方式，
        }
        else
        {//对于底层图像，直接就扩充边界了
            copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
                           BORDER_REFLECT_101);            
        }
    }

}

} //namespace ORB_SLAM

关于copyMakeBorder可参考这个。