以TUM数据集为例,运行Demo的命令:

./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml PATH_TO_SEQUENCE_FOLDER ASSOCIATIONS_FILE

rgbd_tum.cc的源码:

int main(int argc, char **argv) {
    // 判断输入参数个数
    if (argc != 5) {
        cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;
        return 1;
    }
​
    // step1. 读取图片及左右目关联信息
    vector<string> vstrImageFilenamesRGB;
    vector<string> vstrImageFilenamesD;
    vector<double> vTimestamps;
    string strAssociationFilename = string(argv[4]);
    LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);    
    
    // step2. 检查图片文件及输入文件的一致性
    int nImages = vstrImageFilenamesRGB.size();
    if (vstrImageFilenamesRGB.empty()) {
        cerr << endl << "No images found in provided path." << endl;
        return 1;
    } else if (vstrImageFilenamesD.size() != vstrImageFilenamesRGB.size()) {
        cerr << endl << "Different number of images for rgb and depth." << endl;
        return 1;
    }
​
    // step3. 创建SLAM对象,它是一个 ORB_SLAM2::System 类型变量
    ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::RGBD, true);
    
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);
    cv::Mat imRGB, imD;
    // step4. 遍历图片,进行SLAM
    for (int ni = 0; ni < nImages; ni++) {
        // step4.1. 读取图片
        imRGB = cv::imread(string(argv[3]) + "/" + vstrImageFilenamesRGB[ni], CV_LOAD_IMAGE_UNCHANGED);
        imD = cv::imread(string(argv[3]) + "/" + vstrImageFilenamesD[ni], CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];
        // step4.2. 进行SLAM
        SLAM.TrackRGBD(imRGB, imD, tframe);
        // step4.3. 加载下一张图片
        double T = 0;
        if (ni < nImages - 1)
            T = vTimestamps[ni + 1] - tframe;
        else if (ni > 0)
            T = tframe - vTimestamps[ni - 1];
​
        if (ttrack < T)
            usleep((T - ttrack) * 1e6);
    }
​
    // step5. 停止SLAM
    SLAM.Shutdown();
}

运行程序rgbd_tum时传入了一个重要的配置文件TUM1.yaml,其中保存了相机参数和ORB特征提取参数:

%YAML:1.0
​
## 相机参数
Camera.fx: 517.306408
Camera.fy: 516.469215
Camera.cx: 318.643040
Camera.cy: 255.313989
​
Camera.k1: 0.262383
Camera.k2: -0.953104
Camera.p1: -0.005358
Camera.p2: 0.002628
Camera.k3: 1.163314
​
Camera.width: 640
Camera.height: 480
​
Camera.fps: 30.0        # Camera frames per second 
Camera.bf: 40.0         # IR projector baseline times fx (aprox.)
Camera.RGB: 1           # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
ThDepth: 40.0           # Close/Far threshold. Baseline times.
DepthMapFactor: 5000.0  # Deptmap values factor 
​
## ORB特征提取参数
ORBextractor.nFeatures: 1000        # ORB Extractor: Number of features per image
ORBextractor.scaleFactor: 1.2       # ORB Extractor: Scale factor between levels in the scale pyramid 
ORBextractor.nLevels: 8             # ORB Extractor: Number of levels in the scale pyramid  
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

ORB-SLAM2中的变量遵循一套命名规则:

• 变量名的第一个字母为m表示该变量为某类的成员变量.
• 变量名的第一、二个字母表示数据类型:
  • p表示指针类型
  • n表示int类型
  • b表示bool类型
  • s表示std::set类型
  • v表示std::vector类型
  • l表示std::list类型
  • KF表示KeyFrame类型

这种将变量类型写进变量名的命名方法叫做匈牙利命名法.

1. 加快运算速度:

   bool Initializer::Initialize(const Frame &CurrentFrame) {
       // ...
       thread threadH(&Initializer::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H));
       thread threadF(&Initializer::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F));
       // ...
   }
   

   开两个线程同时计算两个矩阵,在多核处理器上会加快运算速度.

2. 因为系统的随机性,各步骤的运行顺序是不确定的.

   Tracking线程不产生关键帧时,LocalMapping和LoopClosing线程基本上处于空转的状态.

   而Tracking线程产生关键帧的频率和时机不是固定的,因此需要3个线程同时运行,LocalMapping和LoopClosing线程不断循环查询Tracking线程是否产生关键帧,产生了的话就处理.

   

// Tracking线程主函数
void Tracking::Track() {
    // 进行跟踪
    // ...
    
    // 若跟踪成功,根据条件判定是否产生关键帧
    if (NeedNewKeyFrame())
        // 产生关键帧并将关键帧传给LocalMapping线程
        KeyFrame *pKF = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
        mpLocalMapper->InsertKeyFrame(pKF); 
}
​
// LocalMapping线程主函数
void LocalMapping::Run() {
    // 死循环
    while (1) {
        // 判断是否接收到关键帧
        if (CheckNewKeyFrames()) {
            // 处理关键帧
            // ...
            
            // 将关键帧传给LoopClosing线程
            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
        }
        
        // 线程暂停3毫秒,3毫秒结束后再从while(1)循环首部运行
        std::this_thread::sleep_for(std::chrono::milliseconds(3));
    }
}
​
// LoopClosing线程主函数
void LoopClosing::Run() {
    // 死循环
    while (1) {
        // 判断是否接收到关键帧
        if (CheckNewKeyFrames()) {
            // 处理关键帧
            // ...
        }
​
        // 查看是否有外部线程请求复位当前线程
        ResetIfRequested();
​
        // 线程暂停5毫秒,5毫秒结束后再从while(1)循环首部运行
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

为防止多个线程同时操作同一变量造成混乱,引入锁机制:

将成员函数本身设为私有变量(private或protected),并在操作它们的公有函数内加锁.

class KeyFrame { 
        
protected:
    KeyFrame* mpParent;
    
public:
    void KeyFrame::ChangeParent(KeyFrame *pKF) { 
        
        unique_lock<mutex> lockCon(mMutexConnections);      // 加锁
        mpParent = pKF;
        pKF->AddChild(this);
    }
​
    KeyFrame *KeyFrame::GetParent() { 
        
        unique_lock<mutex> lockCon(mMutexConnections);      // 加锁
        return mpParent;
    }
}

一把锁在某个时刻只有一个线程能够拿到,如果程序执行到某个需要锁的位置,但是锁被别的线程拿着不释放的话,当前线程就会暂停下来;直到其它线程释放了这个锁,当前线程才能拿走锁并继续向下执行.

• 什么时候加锁和释放锁?

  unique_lock lockCon(mMutexConnections);这句话就是加锁,锁的有效性仅限于大括号{}之内,也就是说,程序运行出大括号之后就释放锁了.因此可以看到有一些代码中加上了看似莫名其妙的大括号.

void KeyFrame::EraseConnection(KeyFrame *pKF) {
    // 第一部分加锁
    {
        unique_lock<mutex> lock(mMutexConnections);
        if (mConnectedKeyFrameWeights.count(pKF)) {
            mConnectedKeyFrameWeights.erase(pKF);
            bUpdate = true;
        }
    }// 程序运行到这里就释放锁,后面的操作不需要抢到锁就能执行
    
    UpdateBestCovisibles();
}

System(const string &strVocFile, string &strSettingsFile, const eSensor sensor, const bool bUseViewer=true): 构造函数

System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor, const bool bUseViewer) : 
        mSensor(sensor), mpViewer(static_cast<Viewer *>(NULL)), mbReset(false), mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false) {
    
    // step1. 初始化各成员变量
    // step1.1. 读取配置文件信息
    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
    // step1.2. 创建ORB词袋
    mpVocabulary = new ORBVocabulary();
    // step1.3. 创建关键帧数据库,主要保存ORB描述子倒排索引(即根据描述子查找拥有该描述子的关键帧)
    mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
    // step1.4. 创建地图
    mpMap = new Map();
​
    // step2. 创建3大线程: Tracking、LocalMapping和LoopClosing
    // step2.1. 主线程就是Tracking线程,只需创建Tracking对象即可
    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
    // step2.2. 创建LocalMapping线程及mpLocalMapper
    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper);
    // step2.3. 创建LoopClosing线程及mpLoopCloser
    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
            
    // step3. 设置线程间通信
    mpTracker->SetLocalMapper(mpLocalMapper);
    mpTracker->SetLoopClosing(mpLoopCloser);
    mpLocalMapper->SetTracker(mpTracker);
    mpLocalMapper->SetLoopCloser(mpLoopCloser);
    mpLoopCloser->SetTracker(mpTracker);
    mpLoopCloser->SetLocalMapper(mpLocalMapper);
}

LocalMapping和LoopClosing线程在System类中有对应的std::thread线程成员变量,为什么Tracking线程没有对应的std::thread成员变量?

因为Tracking线程就是主线程,而LocalMapping和LoopClosing线程是其子线程,主线程通过持有两个子线程的指针(mptLocalMapping和mptLoopClosing)控制子线程.

(ps: 虽然在编程实现上三大主要线程构成父子关系,但逻辑上我们认为这三者是并发的,不存在谁控制谁的问题).

System对象所在的主线程就是跟踪线程,针对不同的传感器类型有3个用于跟踪的函数,其内部实现就是调用成员变量mpTracker的GrabImageMonocular(GrabImageStereo或GrabImageRGBD)方法.

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp) {
    cv::Mat Tcw = mpTracker->GrabImageMonocular(im, timestamp);
    unique_lock<mutex> lock(mMutexState);
    mTrackingState = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
    return Tcw;
}

FAST特征点和ORB描述子本身不具有尺度信息,ORBextractor通过构建图像金字塔来得到特征点尺度信息.将输入图片逐级缩放得到图像金字塔,金字塔层级越高,图片分辨率越低,ORB特征点越大.

构造函数ORBextractor(int nfeatures, float scaleFactor, int nlevels, int iniThFAST, int minThFAST)的流程:

1. 初始化图像金字塔相关变量:

   下面成员变量从配置文件TUM1.yaml中读入:

   成员变量	访问控制	意义	配置文件TUM1.yaml中变量名	值
   int nfeatures	protected	所有层级提取到的特征点数之和金字塔层数	ORBextractor.nFeatures	1000
   double scaleFactor	protected	图像金字塔相邻层级间的缩放系数	ORBextractor.scaleFactor	1.2
   int nlevels	protected	金字塔层级数	ORBextractor.nLevels	8
   int iniThFAST	protected	提取特征点的描述子门槛(高)	ORBextractor.iniThFAST	20
   int minThFAST	protected	提取特征点的描述子门槛(低)	ORBextractor.minThFAST	7

   根据上述变量的值计算出下述成员变量:

   成员变量	访问控制	意义	值
   std::vector mnFeaturesPerLevel	protected	金字塔每层级中提取的特征点数 正比于图层边长,总和为nfeatures	{61, 73, 87, 105, 126, 151, 181, 216}
   std::vector mvScaleFactor	protected	各层级的缩放系数	{1, 1.2, 1.44, 1.728, 2.074, 2.488, 2.986, 3.583}
   std::vector mvInvScaleFactor	protected	各层级缩放系数的倒数	{1, 0.833, 0.694, 0.579, 0.482, 0.402, 0.335, 0.2791}
   std::vector mvLevelSigma2	protected	各层级缩放系数的平方	{1, 1.44, 2.074, 2.986, 4.300, 6.190, 8.916, 12.838}
   std::vector mvInvLevelSigma2	protected	各层级缩放系数的平方倒数	{1, 0.694, 0.482, 0.335, 0.233, 0.162, 0.112, 0.078}

2. 初始化用于计算描述子的pattern变量,pattern是用于计算描述子的256对坐标,其值写死在源码文件ORBextractor.cc里,在构造函数里做类型转换将其转换为const cv::Point*变量.

static int bit_pattern_31_[256*4] ={ 
        
    8,-3, 9,5/*mean (0), correlation (0)*/, 
    4,2, 7,-12/*mean (1.12461e-05), correlation (0.0437584)*/,
    -11,9, -8,2/*mean (3.37382e-05), correlation (0.0617409)*/,
    7,-12, 12,-13/*mean (5.62303e-05), correlation (0.0636977)*/,
    2,-13, 2,12/*mean (0.000134953), correlation (0.085099)*/,
    // 共256行...
}
​
const Point* pattern0 = (const Point*)bit_pattern_31_;
std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));

3. 计算一个半径为16的圆的近似坐标

后面计算的是特征点主方向上的描述子,计算过程中要将特征点周围像素旋转到主方向上,因此计算一个半径为16的圆的近似坐标,用于后面计算描述子时进行旋转操作.

成员变量std::vector umax里存储的实际上是逼近圆的第一象限内圆周上每个v坐标对应的u坐标.为保证严格对称性,先计算下45°圆周上点的坐标,再根据对称性补全上45°圆周上点的坐标.

int vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);    // 45°射线与圆周交点的纵坐标
int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);         // 45°射线与圆周交点的纵坐标
​
// 先计算下半45度的umax
for (int v = 0; v <= vmax; ++v) {
    umax[v] = cvRound(sqrt(15 * 15 - v * v));   
}
​
// 根据对称性补出上半45度的umax
for (int v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v) {
    while (umax[v0] == umax[v0 + 1])
        ++v0;
    umax[v] = v0;
    ++v0;
}

根据上述变量的值计算出下述成员变量:

函数void ORBextractor::ComputePyramid(cv::Mat image)逐层计算图像金字塔,对于每层图像进行以下两步:

1. 先进行图片缩放,缩放到mvInvScaleFactor对应尺寸.
2. 在图像外补一圈厚度为19的padding(提取FAST特征点需要特征点周围半径为3的圆域,计算ORB描述子需要特征点周围半径为16的圆域).

下图表示图像金字塔每层结构:

• 深灰色为缩放后的原始图像.
• 包含绿色边界在内的矩形用于提取FAST特征点.
• 包含浅灰色边界在内的整个矩形用于计算ORB描述子.

void ORBextractor::ComputePyramid(cv::Mat image) {
    for (int level = 0; level < nlevels; ++level) {
        // 计算缩放+补padding后该层图像的尺寸
        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);
        Mat temp(wholeSize, image.type());
        
        // 缩放图像并复制到对应图层并补边
        mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
        if( level != 0 ) {
            resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, cv::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);            
        }
    }
}

copyMakeBorder函数实现了复制和padding填充,其参数BORDER_REFLECT_101参数指定对padding进行镜像填充.

提取特征点最重要的就是力求特征点均匀地分布在图像的所有部分,为实现这一目标,编程实现上使用了两个技巧:

1. 分CELL搜索特征点,若某CELL内特征点响应值普遍较小的话就降低分数线再搜索一遍.
2. 对得到的所有特征点进行八叉树筛选,若某区域内特征点数目过于密集,则只取其中响应值最大的那个.

CELL搜索的示意图如下,每个CELL的大小约为30✖30,搜索到边上,剩余尺寸不够大的时候,最后一个CELL有多大就用多大的区域.

需要注意的是相邻的CELL之间会有6像素的重叠区域,因为提取FAST特征点需要计算特征点周围半径为3的圆周上的像素点信息,实际上产生特征点的区域比传入的搜索区域小3像素.

void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints) {
    for (int level = 0; level < nlevels; ++level)
        // 计算图像边界
        const int minBorderX = EDGE_THRESHOLD-3;        
        const int minBorderY = minBorderX;              
        const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
        const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;
        const float width = (maxBorderX-minBorderX);
        const float height = (maxBorderY-minBorderY);
        const int nCols = width/W;              // 每一列有多少cell
        const int nRows = height/W;             // 每一行有多少cell
        const int wCell = ceil(width/nCols);    // 每个cell的宽度
        const int hCell = ceil(height/nRows);   // 每个cell的高度
​
        // 存储需要进行平均分配的特征点
        vector<cv::KeyPoint> vToDistributeKeys;
        
        // step1. 遍历每行和每列,依次分别用高低阈值搜索FAST特征点
        for(int i=0; i<nRows; i++) {
            const float iniY = minBorderY + i * hCell;
            const float maxY = iniY + hCell + 6;
            for(int j=0; j<nCols; j++) {
                const float iniX =minBorderX + j * wCell;
                const float maxX = iniX + wCell + 6;
                vector<cv::KeyPoint> vKeysCell;
                
                // 先用高阈值搜索FAST特征点
                FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell, iniThFAST, true);
                // 高阈值搜索不到的话,就用低阈值搜索FAST特征点
                if(vKeysCell.empty()) {
                    FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell, minThFAST, true);
                }
                // 把 vKeysCell 中提取到的特征点全添加到 容器vToDistributeKeys 中
                for(KeyPoint point :vKeysCell) {
                    point.pt.x+=j*wCell;
                    point.pt.y+=i*hCell;
                    vToDistributeKeys.push_back(point);
                }
            }
        }
        
        // step2. 对提取到的特征点进行八叉树筛选,见 DistributeOctTree() 函数
        keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX, minBorderY, maxBorderY, mnFeaturesPerLevel[level], level);
    }
    // 计算每个特征点的方向
    for (int level = 0; level < nlevels; ++level)
        computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);               
    }
}

函数DistributeOctTree()进行八叉树筛选(非极大值抑制),不断将存在特征点的图像区域进行4等分,直到分出了足够多的分区,每个分区内只保留响应值最大的特征点.

其代码实现比较琐碎,程序里还定义了一个ExtractorNode类用于进行八叉树分配,知道原理就行,不看代码.

函数computeOrientation()计算每个特征点的方向: 使用特征点周围半径19大小的圆的重心方向作为特征点方向.

static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax)
{
    for (vector<KeyPoint>::iterator keypoint : keypoints) {
        // 调用IC_Angle 函数计算这个特征点的方向
        keypoint->angle = IC_Angle(image, keypoint->pt, umax);  
    }
}
​
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));
    for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
        m_10 += u * center[u];
    int step = (int)image.step1();
    for (int v = 1; v <= HALF_PATCH_SIZE; ++v) {
        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);
            m_10 += u * (val_plus + val_minus);
        }
        m_01 += v * v_sum;
    }
​
    // 为了加快速度使用了fastAtan2()函数，输出为[0,360)角度，精度为0.3°
    return fastAtan2((float)m_01, (float)m_10);
}

计算BRIEF描述子的核心步骤是在特征点周围半径为16的圆域内选取256对点对,每个点对内比较得到1位,共得到256位的描述子,为保计算的一致性,工程上使用特定设计的点对pattern,在程序里被硬编码为成员变量了.

在computeOrientation()中我们求出了每个特征点的主方向,在计算描述子时,应该将特征点周围像素旋转到主方向上来计算;为了编程方便,实践上对pattern进行旋转.

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;
​
    // 旋转公式
    // x'= xcos(θ) - ysin(θ)
    // y'= xsin(θ) + ycos(θ)
    #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) {
        int t0, t1, val;
        t0 = GET_VALUE(0); t1 = GET_VALUE(1);
        val = t0 < t1;                              // 描述子本字节的bit0
        t0 = GET_VALUE(2); t1 = GET_VALUE(3);
        val |= (t0 < t1) << 1;                      // 描述子本字节的bit1
        t0 = GET_VALUE(4); t1 = GET_VALUE(5);
        val |= (t0 < t1) << 2;                      // 描述子本字节的bit2
        t0 = GET_VALUE(6); t1 = GET_VALUE(7);
        val |= (t0 < t1) << 3;                      // 描述子本字节的bit3
        t0 = GET_VALUE(8); t1 = GET_VALUE(9);
        val |= (t0 < t1) << 4;                      // 描述子本字节的bit4
        t0 = GET_VALUE(10); t1 = GET_VALUE(11);
        val |= (t0 < t1) << 5;                      // 描述子本字节的bit5
        t0 = GET_VALUE(12); t1 = GET_VALUE(13);
        val |= (t0 < t1) << 6;                      // 描述子本字节的bit6
        t0 = GET_VALUE(14); t1 = GET_VALUE(15);
        val |= (t0 < t1) << 7;                      // 描述子本字节的bit7
​
        //保存当前比较的出来的描述子的这个字节
        desc[i] = (uchar)val;
    }
}

ORBextractor被用于Tracking线程对输入图像预处理的第一步.

这个函数重载了()运算符,使得其他类可以将ORBextractor类型变量当作函数来使用.

该函数是ORBextractor的主函数,内部依次调用了上面提到的各过程.

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-iRpb6G9u-1651331512006)(…/AppData/Roaming/Typora/typora-user-images/1628640469328.png)]

提取特征点void operator()()计算特征点并进行八叉树筛选 ComputeKeyPointsOctTree()检查图像有效性计算特征点并进行八叉树筛选 ComputeKeyPointsOctTree()遍历每一层图像,计算描述子 computeOrbDescriptor()逐层遍历 按CELL提取FAST特征点调用DistributeOctTree() 筛选特征点,进行非极大值抑制调用computeOrientation() 计算每个特征点的主方向

void ORBextractor::operator()(InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints, OutputArray _descriptors) { 
    // step1. 检查图像有效性
    if(_image.empty())
        return;
    Mat image = _image.getMat();
    assert(image.type() == CV_8UC1 );
​
    // step2. 构建图像金字塔
    ComputePyramid(image);
​
    // step3. 计算特征点并进行八叉树筛选
    vector<vector<KeyPoint> > allKeypoints; 
    ComputeKeyPointsOctTree(allKeypoints);
​
    // step4. 遍历每一层图像,计算描述子
    int offset = 0;
    for (int level = 0; level < nlevels; ++level) {
        Mat workingMat = mvImagePyramid[level].clone();
        // 计算描述子之前先进行一次高斯模糊
        GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
        computeDescriptors(workingMat, allKeypoints[level], descriptors.rowRange(offset, offset + allKeypoints[level].size());, pattern);
        offset += allKeypoints[level].size();
    }
}

这个重载()运算符的用法被用在Frame类的ExtractORB()函数中了,这也是ORBextractor类在整个项目中唯一被调用的地方.

// 函数中`mpORBextractorLeft`和`mpORBextractorRight`都是`ORBextractor`对象
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);
}

• Frame类中与ORBextractor有关的成员变量和成员函数

  成员变量/函数	访问控制	意义
  ORBextractor* mpORBextractorLeft	public	左目特征点提取器
  ORBextractor* mpORBextractorRight	public	右目特征点提取器,单目/RGBD模式下为空指针
  Frame()	public	Frame类的构造函数,其中调用ExtractORB()函数进行特征点提取
  ExtractORB()	public	提取ORB特征点,其中调用了mpORBextractorLeft和mpORBextractorRight的()方法

// Frame类的两个ORBextractor是在调用构造函数时传入的,构造函数中调用ExtractORB()提取特征点
Frame::Frame(ORBextractor *extractorLeft, ORBextractor *extractorRight) 
    : mpORBextractorLeft(extractorLeft), mpORBextractorRight(extractorRight) {
​
        // ...
​
        // 提取ORB特征点
        thread threadLeft(&Frame::ExtractORB, this, 0, imLeft);
        thread threadRight(&Frame::ExtractORB, this, 1, imRight);
        threadLeft.join();
        threadRight.join();
​
        // ...       
    }
​
// 提取特征点
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);
}

Frame类的两个ORBextractor指针指向的变量是Tracking类的构造函数中创建的

// Tracking构造函数
Tracking::Tracking() {
    // ...
    
    // 创建两个ORB特征点提取器
    mpORBextractorLeft = new ORBextractor(nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST);
    if (sensor == System::STEREO)
        mpORBextractorRight = new ORBextractor(nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST);
​
    // ...
}
​
// Tracking线程每收到一帧输入图片,就创建一个Frame对象,创建Frame对象时将提取器mpORBextractorLeft和mpORBextractorRight给构造函数
cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp) {
    // ...
    
    // 创建Frame对象
    mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft, mpORBextractorRight);
    
    // ...
}

• 由上述代码分析可知,每次完成ORB特征点提取之后,图像金字塔信息就作废了,下一帧图像到来时调用ComputePyramid()函数会覆盖掉本帧图像的图像金字塔信息;但从金字塔中提取的图像特征点的信息会被保存在Frame对象中.所以ORB-SLAM2是稀疏重建,对每帧图像只保留最多nfeatures个特征点(及其对应的地图点).

构造函数ORBextractor()初始化图像金字塔相关变量初始化用于计算描述子的pattern计算近似圆形的边界坐标umax

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-4IJssDBk-1651331512006)(…/AppData/Roaming/Typora/typora-user-images/1628640595104.png)]

遍历每个30*30的CELL,依次分别使用高低阈值提取FAST特征点找到特征点找到特征点没找到特征点没找到特征点没遍历完所有CELL遍历完所有CELL使用高响应阈值iniThFAST搜索特征点使用低响应阈值minThFAST搜索特征点记录特征点移动到下一块CELL取第一个CELL调用DistributeOctTree()对上一步找到的所有特征点进行八叉树筛选 对特征点密集区域进行非极大值抑制调用computeOrientation()计算每个特征点的主方向

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-VmLs51xJ-1651331512007)(…/AppData/Roaming/Typora/typora-user-images/1628640638080.png)]

MapPoint的生命周期

成员变量std::map mObservations保存了当前关键点对关键帧KeyFrame的观测关系,std::map是一个key-value结构,其key为某个关键帧,value为当前地图点在该关键帧中的索引(是在该关键帧成员变量std::vector mvpMapPoints中的索引).

成员int nObs记录了当前地图点被多少个关键帧相机观测到了(单目关键帧每次观测算1个相机,双目/RGBD帧每次观测算2个相机).

• 函数AddObservation()和EraseObservation()同时维护mObservations和nObs

// 向参考帧pKF中添加对本地图点的观测,本地图点在pKF中的编号为idx
void MapPoint::AddObservation(KeyFrame* pKF, size_t idx) {
    unique_lock<mutex> lock(mMutexFeatures);
    // 如果已经添加过观测，返回
    if(mObservations.count(pKF)) 
        return;
    // 如果没有添加过观测，记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引
    mObservations[pKF]=idx;
​
    // 根据观测形式是单目还是双目更新观测计数变量nObs
    if(pKF->mvuRight[idx]>=0)
        nObs += 2; 
    else
        nObs++; 
}

// 从参考帧pKF中移除本地图点
void MapPoint::EraseObservation(KeyFrame* pKF)  {
    bool bBad=false;
    {
        unique_lock<mutex> lock(mMutexFeatures);
        // 查找这个要删除的观测,根据单目和双目类型的不同从其中删除当前地图点的被观测次数
        if(mObservations.count(pKF)) {
            if(pKF->mvuRight[mObservations[pKF]]>=0)
                nObs-=2;
            else
                nObs--;
​
            mObservations.erase(pKF);
​
            // 如果该keyFrame是参考帧，该Frame被删除后重新指定RefFrame
            if(mpRefKF == pKF)
                mpRefKF = mObservations.begin()->first;     // ????参考帧指定得这么草率真的好么?
​
            // 当观测到该点的相机数目少于2时，丢弃该点(至少需要两个观测才能三角化)
            if(nObs<=2)
                bBad=true;
        }
    }
​
    if(bBad)
        // 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
        SetBadFlag();
}

函数GetIndexInKeyFrame()和IsInKeyFrame()就是对mObservations的简单查询

int MapPoint::GetIndexInKeyFrame(KeyFrame *pKF) {
    unique_lock<mutex> lock(mMutexFeatures);
    if(mObservations.count(pKF))
        return mObservations[pKF];
    else
        return -1;
}
​
bool MapPoint::IsInKeyFrame(KeyFrame *pKF) {
    unique_lock<mutex> lock(mMutexFeatures);
    return (mObserva