ORB_SLAM2学习笔记（2）——特征提取

ORB_SLAM2学习笔记（2）——特征提取

本文主要参考slam十四讲，顺序参考一步步带你看懂orbslam2源码

下面函数为单目Tracking线程的接口（文件momo_tum.cc中）：

        // Pass the image to the SLAM system整个系统的核心所在,负责实时跟踪输入图片,
        SLAM.TrackMonocular(im,tframe);//输入的是图片和时间

下面为图形控制界面，控制是否激活定位模式和是否系统复位功能

代码如下（文件System.cc中）：

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp) //单目前系统 同文件夹下还有双目以及RGBD的代码实现
{
    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;//确保只执行一次
       }
    }

下面函数为开始进行ORB特征提取（文件Frame.cc中）：

// ORB extraction
ExtractORB(0,imGray);

转到该函数定义，这里用到了一个重载过的括号运算符：

void Frame::ExtractORB(int flag, const cv::Mat &im)
{
    // mpORBextractorLeft是ORBextractor对象，因为ORBextractor重载了()
    // 所以才会有下面这种用法
    if(flag==0)
        (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);
    else
        (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}

下面为重载运算符中，提取特征点和描述子的流程：

主要就是构建金字塔，然后提取特征点和描述子，然后根据金字塔的结构依次映射在每一层中，为后面特征点匹配作准备。

图像金字塔的构建
第0层为最下面一层，随着金字塔层数的增多，逐渐模糊（文件ORBextractor.cc中）

void ORBextractor::ComputePyramid(cv::Mat image)
{
    for (int level = 0; level < nlevels; ++level)//遍历金字塔每一层
    {
        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));
        //按照scale比例缩放后,定义的相应大小的Mat
        //Rect(矩形左上角横坐标,纵坐标以及矩形的宽度,高度),每经过一次循环,照片大小将按scale缩放,creat pre-created dist image
        // Compute the resized image
        if( level != 0 )
        {
            //mvImagePyramid[1]开始，都是由上一层的图像的尺寸得到
            // dsize = Size(round(fx*src.cols), round(fy*src.rows))
            //dsize是输出图像的大小，按照上面的计算公式计算得到了已经
            //resize(InputArry src, Output dst, Size dsize, double fx = 0, double fy = 0, 
            //int interpolation = INTER_LINEAR)
            //将设置出的图像分别拷贝到相应的层上去
            //根据上一层金字塔进行缩放
            resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 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);   //原图不需要缩放   temp和mvImagePyramid[0]共用的是同一段地址，temp和mvImagePyramid[0]中的任何一个发生了变化，都会导致共用的内存地址中的内容发生变化。      
        }
    }

}

下面是一个简化版金字塔结构

void ORBextractor::ComputePyramid_brief(cv::Mat image) {
   for (int level = 0; level < nlevels; ++level) {
        float scale = mvInvScaleFactor[level];
        Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));

        // Compute the resized image
        if (level != 0) {
            resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);
       } else {
            mvImagePyramid[level] = image;
        }
    }
}

Fast 关键点的提取
下面函数进入计算关键点并进行四叉树存储的（文件ORBextractor.cc中）：

   //关键点全部存在allKeypoints中
    vector < vector<KeyPoint> > allKeypoints;
    ComputeKeyPointsOctTree(allKeypoints);

转到ComputeKeyPointsOctTree 的函数（流程就是划分网格，提取fast关键点，转化为整个图像坐标，保存）：

//函数的三要素是：函数名称，函数参数， 函数返回值
//ComputeKeyPointsOctTree是类ORBextractor的成员函数
//参数是vector类型的引用变量allKeypoints.
//返回值是void类型
//在参数文件TUM1.yaml下预定义了一些变量的值
//ORBextractor. nFeatures: 1000
//ORBextractor. scaleFactor: 1.2
//ORBextractor. nlevels: 8
//ORBextractor. iniThFAST: 20
//ORBextractor. minThFAST: 7
//在ORBextractor.h中用带参构造函数来初始化类ORBextractor中的相应的变量
//在类ORBextractor中还有一串变量
//std::vector<float> mvScaleFactor;
//std::vector<float> mvInvScaleFactor;
//std::vector<float> mvlevelSifma2;
//std::vector<float> mvInvLevelSigma2;
//如果说第一组变量是金字塔中某一层图像的属性，那么第二组是成员变量是一幅图像的属性。
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
{
    allKeypoints.resize(nlevels);//通过vector中的resize函数来重新将vector变量allKeypoints的大小设置为nlevels.

    const float W = 30;//定义了网格的大小,实际大小会根据图片大小而定,看下文


    for (int level = 0; level < nlevels; ++level)//一张图片,遍历整个金字塔
    {
        const int minBorderX = EDGE_THRESHOLD-3;//x,y轴边界阈值为16个像素
        const int minBorderY = minBorderX;
        const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;//整行——最小就是最大的X
        const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;
        //vector中存储的数据类型是在opencv中定义的KeyPoint类
        vector<cv::KeyPoint> vToDistributeKeys;
        //reserve：分配空间，更改capacity但是不改变size 预留空间
        //resize：分配空间，同时改变capacity和size
        vToDistributeKeys.reserve(nfeatures*10);
        //计算出除去不考虑的边缘外,图片的width和height
        //预将图像划分为30*30的网状
        //计算每个小格子的长和宽各占多少个像素
        const float width = (maxBorderX-minBorderX);
        const float height = (maxBorderY-minBorderY);
        //计算最终长和宽被分成了多少个小格子cell
        const int nCols = width/W;//网格列,实际上可以值为21.6,然后取21,最多分为21个
        const int nRows = height/W;//网格行
        const int wCell = ceil(width/nCols);//向上取整,大于该值的最小整数,计算出实际上每个网格的宽
        const int hCell = ceil(height/nRows);//计算出实际上每个网格的高


        for(int i=0; i<nRows; i++)//纵向遍历所有网格
        {
            const float iniY =minBorderY+i*hCell;
            float maxY = iniY+hCell+6;//给像素点加一个外框，保证每个像素点都被遍历

            if(iniY>=maxBorderY-3)//超出图片Y轴范围
                continue;
            if(maxY>maxBorderY)//确保maxY的上限
                maxY = maxBorderY;

            for(int j=0; j<nCols; j++)//横向遍历
            {
                const float iniX =minBorderX+j*wCell;
                float maxX = iniX+wCell+6;
                if(iniX>=maxBorderX-6)
                    continue;
                if(maxX>maxBorderX)
                    maxX = maxBorderX;
                //vkeyscell用来放置提取的关键点
                vector<cv::KeyPoint> vKeysCell;
                //变量i和j的组合控制，当遍历到（i, j）个cell时，就提取这个cell下的FAST角点
                //如下是opencv中FAST函数的原型
                //输入图像，输出提取的特征点， 选取特征点的阈值
                ///FAST( InputArray image, CV_OUT vector<KeyPoint>& keypoints,
                /// int threshold, bool nonmaxSuppression=true );
                FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
                     vKeysCell,iniThFAST,true);//提取FAST关键点,采用非极大值抑制,采用初始阈值提取FAST
                /*ORBextractor.iniThFAST: 20
                  ORBextractor.minThFAST: 7
                  位于配置表ORB_SLAM2/Examples/Monocular/TUM2.yaml中*/


                if(vKeysCell.empty())//如果初始阈值提取不到FAST,则采用minTHFAST阈值提取
                {
                    FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
                         vKeysCell,minThFAST,true);
                }

                if(!vKeysCell.empty())//将每个网格中的FAST关键点坐标转换为实际图像中(不包含边界)的坐标,并存入vToDistributeKeys
                {
                    for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
                    //KeyPoint是opencv中的一个类,pt是该类中的一个属性，获取获取关键点的坐标
                    //因为单纯的(*vit).pt.x和(*vit).pt.y表示在当前cell下的坐标，还要转化为在可提取特征范围内的坐标
                    { 
                        (*vit).pt.x+=j*wCell;//计算出对应的x坐标
                        (*vit).pt.y+=i*hCell;
                        vToDistributeKeys.push_back(*vit);//存好关键点
                    }
                }

            }
        }

上述代码已经提取完对应金字塔层数的FAST关键点，并进行四叉树存储，下面代码实现在四叉树筛选出高质量关键点：

        //vector<vector<KeyPoint> >& allKeypoints
        //allKeypoints是一个用来存储vector的vector
        //allKeypoints的大小是金字塔的层数nlevels
        //allKeypoints[level]是一个对应于每层图像上提取的特征点的vector
        //allKeypoints[level].size也就是在该层上要提取的特征点的个数
        vector<KeyPoint> & keypoints = allKeypoints[level];
        keypoints.reserve(nfeatures);
        //将图片进行四叉树存储,筛选高质量关键点,确保均匀，所有提取的关键点，提取的范围，是从哪一层上提取的特征
        keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
                                      minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);
        ///PATCH_SIZE指代什么呢level=0表示原图像，随着层数的增加图像越来越小，那么在每幅图像上提取的特征个数
        //也会相应的减少
        // PATCH_SIZE = 31.
        //vector变量mvScaleFactor中存储了一幅图像对应的一个金字塔中所有层图像的尺度因子
        //不同层图像的尺度因子不同，那么在该层中提取的特征点所对应的有效区域就不同。
        const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];

        // Add border to coordinates and scale information
        const int nkps = keypoints.size();
        for(int i=0; i<nkps ; i++)//记录关键点在图片实际坐标(包含边界),记录该金字塔level
        {
                    {
        //遍历在该层图像上提取的所有的特征点，在这些特征点坐标上都加上整幅图像的边界信息就可以
        //得到关键点在整幅图像中的坐标
            keypoints[i].pt.x+=minBorderX;
            keypoints[i].pt.y+=minBorderY;
            keypoints[i].octave=level;//octave存储金字塔层数
            keypoints[i].size = scaledPatchSize;//不同层所提取的范围不同
        }
    }

    // compute orientations 
    // 遍历每一层图像以及在该层上提取的特征点，计算每个特征点的方向。
    for (int level = 0; level < nlevels; ++level)
    //对应的塔层，FAST关键点，umax可以表示像素点到区域块边界水平距离
        computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
}

计算FAST关键点的方向计算公式：

计算FAST关键点的方向：（这个只是调用，具体在这个下方）

static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax)
{
    for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
         keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
    {
        keypoint->angle = IC_Angle(image, keypoint->pt, umax);
    }
}

IC_Angle函数的程序如下：

static float IC_Angle(const Mat& image, Point2f pt,  const vector<int> & u_max)
{
    int m_01 = 0, m_10 = 0;

    const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));//计算灰度值的公式

    // Treat the center line differently, v=0
    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
        m_10 += u * center[u];//计算y轴上的纵坐标 * 灰度值 之和

    // Go line by line in the circuI853lar patch
    int step = (int)image.step1();//给一个步长
    for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
    {
        // Proceed over the two lines
        int v_sum = 0;
        int d = u_max[v];//为轴线到边界的距离
        for (int u = -d; u <= d; ++u)
        {
            int val_plus = center[u + v*step], val_minus = center[u - v*step];//遍历所有点得到灰度值
            v_sum += (val_plus - val_minus);//x轴只选了一侧 ，无正负，需要加负号
            m_10 += u * (val_plus + val_minus);//纵坐标 * 各点灰度值 求和 一个纵坐标对应两个灰度值
        }
        m_01 += v * v_sum;//横坐标 * 各点灰度值 求和
    }

    return fastAtan2((float)m_01, (float)m_10);//求角度
}

经过筛选，关键点将被存放在Keypoints向量中，四叉树存储思路如下：

首先根据图片的wide/height比值（取整数），将图片竖着划分为若干个根节点：

    // Compute how many initial nodes //要求图片width/height≥0.5,TUM数据集nIni=  
    //首先根据图片的 width/height 比值(四舍五入后的比值),将图片竖着划分为若干个根节点
    const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));

下面函数为四叉树选择最大响应点并储存的过程，流程（比较各个节点下的关键点的响应值代表该节点进行存储）；另外四叉树可以避免提取到的关键点有些地方密集有些地方稀疏，使得计算描述子出现困难：

    // Retain the best point in each node  每个节点中保存最好的点作为大哥哥
    vector<cv::KeyPoint> vResultKeys;
    //reserve：分配空间，更改capacity但是不改变size 预留空间
    vResultKeys.reserve(nfeatures);
    //迭代器指针lit遍历四叉树中所有节点
    for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
    { 
        
        vector<cv::KeyPoint> &vNodeKeys = lit->vKeys;//vNodeKeys指向每个节点中的FAST关键点
        cv::KeyPoint* pKP = &vNodeKeys[0];
        float maxResponse = pKP->response;//先后使得第一个为最强的点，后面可能会替换

        for(size_t k=1;k<vNodeKeys.size();k++)//遍历节点中每一个点
        {
            if(vNodeKeys[k].response>maxResponse)//用其他强壮的点替换初始化的强壮点
            {
                pKP = &vNodeKeys[k];
                maxResponse = vNodeKeys[k].response;
            }
        }

        vResultKeys.push_back(*pKP);//将每个节点中响应值最大的节点存入vResultKeys，完成四叉树的保存
    }

    return vResultKeys;
}

BRIEF描述子的计算

//计算描述子
static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors,
                               const vector<Point>& pattern)
{
    //创建一个描述子矩阵
    descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);//cv_8UC1 8个字节 无符号整形 单通道灰度图像

    for (size_t i = 0; i < keypoints.size(); i++)
        computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i));//输入关键点 图像 点对 描述子矩阵指针
}

转到computeOrbDescriptor函数看下，按照一定的规则进行组成点对，然后比较灰度值，进行赋值1或0 的操作用于后面匹配（用汉明距离进行匹配例如两幅图片四组点对分别为1010与1111 汉明距离就是2，这个数字小于多时候就认定匹配成功，这里用的是256对点对）：

const float factorPI = (float)(CV_PI/180.f);
static void computeOrbDescriptor(const KeyPoint& kpt,
                                 const Mat& img, const Point* pattern,
                                 uchar* desc)
{
    float angle = (float)kpt.angle*factorPI;
    float a = (float)cos(angle), b = (float)sin(angle);

    const uchar* center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
    const int step = (int)img.step;

    #define GET_VALUE(idx) \
        center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \
               cvRound(pattern[idx].x*a - pattern[idx].y*b)]//计算灰度值


    for (int i = 0; i < 32; ++i, pattern += 16)//一次处理16组
    {
        int t0, t1, val;
        t0 = GET_VALUE(0); t1 = GET_VALUE(1);
        val = t0 < t1;
        t0 = GET_VALUE(2); t1 = GET_VALUE(3);
        val |= (t0 < t1) << 1;
        t0 = GET_VALUE(4); t1 = GET_VALUE(5);
        val |= (t0 < t1) << 2;
        t0 = GET_VALUE(6); t1 = GET_VALUE(7);
        val |= (t0 < t1) << 3;
        t0 = GET_VALUE(8); t1 = GET_VALUE(9);
        val |= (t0 < t1) << 4;
        t0 = GET_VALUE(10); t1 = GET_VALUE(11);
        val |= (t0 < t1) << 5;
        t0 = GET_VALUE(12); t1 = GET_VALUE(13);
        val |= (t0 < t1) << 6;
        t0 = GET_VALUE(14); t1 = GET_VALUE(15);
        val |= (t0 < t1) << 7;

        desc[i] = (uchar)val;
    }

    #undef GET_VALUE
}

然后就是对特征点进行一些处理：
去畸变（文件Frame.cc中）：

UndistortKeyPoints();//去畸变

void Frame::UndistortKeyPoints()
{
    //不需要去畸变的点直接传到mvkeysun中
    if(mDistCoef.at<float>(0)==0.0)
    {
        mvKeysUn=mvKeys;
        return;
    }

    // Fill matrix with points
    cv::Mat mat(N,2,CV_32F);//N行 2列  存储类型为32位Float
    for(int i=0; i<N; i++)  //遍历所以点，进行存储到mat
    {
        mat.at<float>(i,0)=mvKeys[i].pt.x;
        mat.at<float>(i,1)=mvKeys[i].pt.y;
    }

    // Undistort points
    //C++: Mat Mat::reshape(int cn, int rows=0) const cn表示通道数 row表示行数
    mat=mat.reshape(2);//变为2通道
    cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);//调用opencv进行去畸变处理
    mat=mat.reshape(1);//变为1通道

    // Fill undistorted keypoint vector
    //把去畸变后的点存在mvkeysun中
    mvKeysUn.resize(N);
    for(int i=0; i<N; i++)
    {
        cv::KeyPoint kp = mvKeys[i];
        kp.pt.x=mat.at<float>(i,0);
        kp.pt.y=mat.at<float>(i,1);
        mvKeysUn[i]=kp;
    }
}

网格化：将特征点坐标转化为网格坐标，并判断该坐标是否在网格坐标范围内，是的话存入网格，不是抛弃（文件Frame.cc中）：

AssignFeaturesToGrid();//网格化 将特征点坐标转化为网格坐标，并判断该坐标是否在网格坐标范围内，是的话存入网格，不是抛弃

void Frame::AssignFeaturesToGrid()
{
    int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS); //设置每个网格大小 FRAME_GRID_ROWS 48 FRAME_GRID_COLS 64
    for(unsigned int i=0; i<FRAME_GRID_COLS;i++)//进行网格划分
        for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
            mGrid[i][j].reserve(nReserve);//生成网格

    for(int i=0;i<N;i++)
    {
        const cv::KeyPoint &kp = mvKeysUn[i];//取出去畸变的关键点

        int nGridPosX, nGridPosY;//网格坐标系
        if(PosInGrid(kp,nGridPosX,nGridPosY)) //如果该关键点在网格里就保存
            mGrid[nGridPosX][nGridPosY].push_back(i);
    }
}