ORB-SLAM2代码详解

ORB-SLAM2代码详解

1. ORB-SLAM2代码详解01_ORB-SLAM2代码运行流程

1 运行官方Demo

以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

1.2. 阅读代码之前你应该知道的事情

1.2.1 变量命名规则

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

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

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

1.3 理解多线程

1.3.1 为什么要使用多线程?

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

1.3.2 多线程中的锁

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

将成员函数本身设为私有变量(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();
}

1.4 SLAM主类System

1.4.1 System`类是ORB-SLAM2系统的主类,先分析其主要的成员函数和成员变量:

成员变量/函数	访问控制	意义
eSensor mSensor	private	传感器类型MONOCULAR,STEREO,RGBD
ORBVocabulary* mpVocabulary	private	ORB字典,保存ORB描述子聚类结果
KeyFrameDatabase* mpKeyFrameDatabase	private	关键帧数据库,保存ORB描述子倒排索引
Map* mpMap	private	地图
Tracking* mpTracker	private	追踪器
LocalMapping* mpLocalMapper std::thread* mptLocalMapping	private private	局部建图器 局部建图线程
LoopClosing* mpLoopCloser std::thread* mptLoopClosing	private private	回环检测器 回环检测线程
Viewer* mpViewer FrameDrawer* mpFrameDrawer MapDrawer* mpMapDrawer std::thread* mptViewer	private private private private	查看器 帧绘制器 地图绘制器 查看器线程
System(const string &strVocFile, string &strSettingsFile, const eSensor sensor, const bool bUseViewer=true)	public	构造函数
cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp) cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp) cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp) int mTrackingState std::mutex mMutexState	public public public private private	跟踪双目相机,返回相机位姿 跟踪RGBD相机,返回相机位姿 跟踪单目相机,返回相机位姿 追踪状态 追踪状态锁
bool mbActivateLocalizationMode bool mbDeactivateLocalizationMode std::mutex mMutexMode void ActivateLocalizationMode() void DeactivateLocalizationMode()	private private private public public	开启/关闭纯定位模式
bool mbReset std::mutex mMutexReset void Reset()	private private public	系统复位
void Shutdown()	public	系统关闭
void SaveTrajectoryTUM(const string &filename) void SaveKeyFrameTrajectoryTUM(const string &filename) void SaveTrajectoryKITTI(const string &filename)	public public public	以TUM/KITTI格式保存相机运动轨迹和关键帧位姿

1.4.2 构造函数

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

1.4.3 跟踪函数

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

传感器类型	用于跟踪的成员函数
MONOCULAR	cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)
STEREO	cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp)
RGBD	cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp)

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

2. ORB-SLAM2代码详解02_特征点提取器ORBextractor

2.1各成员函数/变量

2.1.1 构造函数: ORBextractor()

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

2.2 构建图像金字塔: ComputePyramid()

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

成员变量	访问控制	意义
std::vector mvImagePyramid	public	图像金字塔每层的图像
const int EDGE_THRESHOLD	全局变量	为计算描述子和提取特征点补的padding厚度

函数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进行镜像填充.

2.3 提取特征点并进行筛选: ComputeKeyPointsOctTree()

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

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

2.4 八叉树筛选特征点: DistributeOctTree()

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

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

2.5 计算特征点方向computeOrientation()

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

2.6 计算特征点描述子computeOrbDescriptor()

计算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;
    }
}

2.7 ORBextractor类的用途

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

2.7.1 ORBextractor类提取特征点的主函数void operator()()

这个函数重载了()运算符,使得其他类可以将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);
}

2.8 ORBextractor类与其它类间的关系

• 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)]

3. ORB-SLAM2代码详解03_地图点MapPoint

MapPoint的生命周期

3.1 各成员函数/变量

3.1.1 地图点的世界坐标: mWorldPos

成员函数/变量	访问控制	意义
cv::Mat mWorldPos	protected	地图点的世界坐标
cv::Mat GetWorldPos()	public	mWorldPos的get方法
void SetWorldPos(const cv::Mat &Pos)	public	mWorldPos的set方法
std::mutex mMutexPos	protected	mWorldPos的锁

3.1.2 与关键帧的观测关系: mObservations

成员函数/变量	访问控制	意义
std::map mObservations	protected	当前地图点在某KeyFrame中的索引
map GetObservations()	public	mObservations的get方法
void AddObservation(KeyFrame* pKF,size_t idx)	public	添加当前地图点对某KeyFrame的观测
void EraseObservation(KeyFrame* pKF)	public	删除当前地图点对某KeyFrame的观测
bool IsInKeyFrame(KeyFrame* pKF)	public	查询当前地图点是否在某KeyFrame中
int GetIndexInKeyFrame(KeyFrame* pKF)	public	查询当前地图点在某KeyFrame中的索引
int nObs	public	记录当前地图点被多少相机观测到 单目帧每次观测加1,双目帧每次观测加2
int Observations()	public	nObs的get方法

成员变量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 (mObservations.count(pKF));
}

3.2 观测尺度

成员函数/变量	访问控制	意义
cv::Mat mNormalVector	protected	平均观测方向
float mfMinDistance	protected	平均观测距离的下限
float mfMaxDistance	protected	平均观测距离的上限
cv::Mat GetNormal()	public	mNormalVector的get方法
float GetMinDistanceInvariance()	public	mfMinDistance的get方法
float GetMaxDistanceInvariance()	public	mNormalVector的get方法
void UpdateNormalAndDepth()	public	更新平均观测距离和方向
int PredictScale(const float &currentDist, KeyFrame* pKF) int PredictScale(const float &currentDist, Frame* pF)	public public	估计当前地图点在某Frame中对应特征点的金字塔层级
KeyFrame* mpRefKF	protected	当前地图点的参考关键帧
KeyFrame* GetReferenceKeyFrame()	public	mpRefKF的get方法

### 3.2.1 平均观测距离: mfMinDistance和mfMaxDistance

特征点的观测距离与其在图像金字塔中的图层呈线性关系.直观上理解,如果一个图像区域被放大后才能识别出来,说明该区域的观测深度较深.

特征点的平均观测距离的上下限由成员变量mfMaxDistance和mfMinDistance表示:

• mfMaxDistance表示若地图点匹配在某特征提取器图像金字塔第7层上的某特征点,观测距离值
• mfMinDistance表示若地图点匹配在某特征提取器图像金字塔第0层上的某特征点,观测距离值

这两个变量是基于地图点在其参考关键帧上的观测得到的.

// pFrame是当前MapPoint的参考帧
const int level = pFrame->mvKeysUn[idxF].octave;
const float levelScaleFactor = pFrame->mvScaleFactors[level];
const int nLevels = pFrame->mnScaleLevels;
mfMaxDistance = dist*levelScaleFactor;                              
mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1];    

函数int PredictScale(const float &currentDist, KeyFrame* pKF)和int PredictScale(const float &currentDist, Frame* pF)根据某地图点到某帧的观测深度估计其在该帧图片上的层级,是上述过程的逆运算.

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

int MapPoint::PredictScale(const float &currentDist, KeyFrame* pKF) {
    float ratio;
    {
        unique_lock<mutex> lock(mMutexPos);
        ratio = mfMaxDistance/currentDist;
    }
​
    int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
    if(nScale<0)
        nScale = 0;
    else if(nScale>=pKF->mnScaleLevels)
        nScale = pKF->mnScaleLevels-1;
​
    return nScale;
}

3.3 更新平均观测方向和距离: UpdateNormalAndDepth()

函数UpdateNormalAndDepth()更新当前地图点的平均观测方向和距离,其中平均观测方向是根据mObservations中所有观测到本地图点的关键帧取平均得到的;平均观测距离是根据参考关键帧得到的.

void MapPoint::UpdateNormalAndDepth() {
    // step1. 获取地图点相关信息
    map<KeyFrame *, size_t> observations;
    KeyFrame *pRefKF;
    cv::Mat Pos;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
​
        observations = mObservations;
        pRefKF = mpRefKF;            
        Pos = mWorldPos.clone();    
    }
​
    // step2. 根据观测到但钱地图点的关键帧取平均计算平均观测方向
    cv::Mat normal = cv::Mat::zeros(3, 1, CV_32F);
    int n = 0;
    for (KeyFrame *pKF : observations.begin()) {
        normal = normal + normali / cv::norm(mWorldPos - pKF->GetCameraCenter());
        n++;
    }
​
    // step3. 根据参考帧计算平均观测距离
    cv::Mat PC = Pos - pRefKF->GetCameraCenter();       
    const float dist = cv::norm(PC);                    
    const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
    const float levelScaleFactor = pRefKF->mvScaleFactors[level];   
    const int nLevels = pRefKF->mnScaleLevels;                      
​
    {
        unique_lock<mutex> lock3(mMutexPos);
        mfMaxDistance = dist * levelScaleFactor;
        mfMinDistance = mfMaxDistance / pRefKF->mvScaleFactors[nLevels - 1];
        mNormalVector = normal / n;
    }
}

地图点的平均观测距离是根据其参考关键帧计算的,那么参考关键帧KeyFrame* mpRefKF是如何指定的呢?

• 构造函数中,创建该地图点的参考帧被设为参考关键帧.

• 若当前地图点对参考关键帧的观测被删除(EraseObservation(KeyFrame* pKF)),则取第一个观测到当前地图点的关键帧做参考关键帧.

函数MapPoint::UpdateNormalAndDepth()的调用时机:

1. 创建地图点时调用UpdateNormalAndDepth()初始化其观测信息.

   
   pNewMP->AddObservation(pKF, i);
   pKF->AddMapPoint(pNewMP, i);
   pNewMP->ComputeDistinctiveDescriptors();
   pNewMP->UpdateNormalAndDepth();             // 更新平均观测方向和距离  
   mpMap->AddMapPoint(pNewMP);
   

2. 地图点对关键帧的观测mObservations更新时(跟踪局部地图添加或删除对关键帧的观测时、LocalMapping线程删除冗余关键帧时或**LoopClosing线程闭环矫正**时),调用UpdateNormalAndDepth()初始化其观测信息.

   pMP->AddObservation(mpCurrentKeyFrame, i);
   pMP->UpdateNormalAndDepth();
   

3. 地图点世界坐标mWorldPos发生变化时(BA优化之后),调用UpdateNormalAndDepth()初始化其观测信息.

   pMP->SetWorldPos(cvCorrectedP3Dw);
   pMP->UpdateNormalAndDepth();
   

总结成一句话: 只要地图点本身或关键帧对该地图点的观测发生变化,就应该调用函数MapPoint::UpdateNormalAndDepth()更新其观测尺度和方向信息.

3.4 特征描述子

成员函数/变量	访问控制	意义
cv::Mat mDescriptor	protected	当前关键点的特征描述子(所有描述子的中位数)
cv::Mat GetDescriptor()	public	mDescriptor的get方法
void ComputeDistinctiveDescriptors()	public	计算mDescriptor

一个地图点在不同关键帧中对应不同的特征点和描述子,其特征描述子mDescriptor是其在所有观测关键帧中描述子的中位数(准确地说,该描述子与其他所有描述子的中值距离最小).

• 特征描述子的更新时机:

  一旦某地图点对关键帧的观测mObservations发生改变,就调用函数MapPoint::ComputeDistinctiveDescriptors()更新该地图点的特征描述子.

• 特征描述子的用途:

  在函数ORBmatcher::SearchByProjection()和ORBmatcher::Fuse()中,通过比较地图点的特征描述子与图片特征点描述子,实现将地图点与图像特征点的匹配(3D-2D匹配).

3.5 地图点的删除与替换

成员函数/变量	访问控制	意义
bool mbBad	protected	坏点标记
bool isBad()	public	查询当前地图点是否被删除(本质上就是查询mbBad)
void SetBadFlag()	public	删除当前地图点
MapPoint* mpReplaced	protected	用来替换当前地图点的新地图点
void Replace(MapPoint *pMP)	public	使用地图点pMP替换当前地图点

3.6 地图点的删除: SetBadFlag()

变量mbBad用来表征当前地图点是否被删除.

删除地图点的各成员变量是一个较耗时的过程,因此函数SetBadFlag()删除关键点时采取先标记再清除的方式,具体的删除过程分为以下两步:

• 先将坏点标记mbBad置为true,逻辑上删除该地图点.(地图点的社会性死亡)
• 再依次清空当前地图点的各成员变量,物理上删除该地图点.(地图点的肉体死亡)

这样只有在设置坏点标记mbBad时需要加锁,之后的操作就不需要加锁了.

void MapPoint::SetBadFlag() {
    map<KeyFrame *, size_t> obs;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        mbBad = true;           // 标记mbBad,逻辑上删除当前地图点
        obs = mObservations;
        mObservations.clear();
    }
    
    // 删除关键帧对当前地图点的观测
    for (KeyFrame *pKF : obs.begin()) {
        pKF->EraseMapPointMatch(mit->second);
    }
​
    // 在地图类上注册删除当前地图点,这里会发生内存泄漏
    mpMap->EraseMapPoint(this);
}

成员变量mbBad表示当前地图点逻辑上是否被删除,在后面用到地图点的地方,都要通过isBad()函数确认当前地图点没有被删除,再接着进行其它操作.

int KeyFrame::TrackedMapPoints(const int &minObs) {
    // ...
    
    
    for (int i = 0; i < N; i++) {
        MapPoint *pMP = mvpMapPoints[i];
        if (pMP && !pMP->isBad()) {         // 依次检查该地图点物理上和逻辑上是否删除,若删除了就不对其操作
            // ...
        }
    }
    
    // ...
}

3.7 地图点的替换: Replace()

函数Replace(MapPoint* pMP)将当前地图点的成员变量叠加到新地图点pMP上.

void MapPoint::Replace(MapPoint *pMP) {
    // 如果是同一地图点则跳过
    if (pMP->mnId == this->mnId)
        return;
​
    // step1. 逻辑上删除当前地图点
    int nvisible, nfound;
    map<KeyFrame *, size_t> obs;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        obs = mObservations;
        mObservations.clear();
        mbBad = true;
        nvisible = mnVisible;
        nfound = mnFound;
        mpReplaced = pMP;
    }
​
    // step2. 将当地图点的数据叠加到新地图点上
    for (map<KeyFrame *, size_t>::iterator mit = obs.begin(), mend = obs.end(); mit != mend; mit++) {
        KeyFrame *pKF = mit->first;
        if (!pMP->IsInKeyFrame(pKF)) {
            pKF->ReplaceMapPointMatch(mit->second, pMP);
            pMP->AddObservation(pKF, mit->second);
        } else {
            pKF->EraseMapPointMatch(mit->second);
        }
    }
​
    pMP->IncreaseFound(nfound);
    pMP->IncreaseVisible(nvisible);
    pMP->ComputeDistinctiveDescriptors();
​
    // step3. 删除当前地图点
    mpMap->EraseMapPoint(this);
}

3.8 MapPoint类的用途

MapPoint的生命周期

针对MapPoint的生命周期,我们关心以下3个问题:

• 创建MapPoint的时机:
  1. Tracking线程中初始化过程(Tracking::MonocularInitialization()和Tracking::StereoInitialization())
  2. Tracking线程中创建新的关键帧(Tracking::CreateNewKeyFrame())
  3. Tracking线程中恒速运动模型跟踪(Tracking::TrackWithMotionModel())也会产生临时地图点,但这些临时地图点在跟踪成功后会被马上删除(那跟踪失败怎么办?跟踪失败的话不会产生关键帧,这些地图点也不会被注册进地图).
  4. LocalMapping线程中创建新地图点的步骤(LocalMapping::CreateNewMapPoints())会将当前关键帧与前一关键帧进行匹配,生成新地图点.
• 删除MapPoint的时机:
  1. LocalMapping线程中删除恶劣地图点的步骤(LocalMapping::MapPointCulling()).
  2. 删除关键帧的函数KeyFrame::SetBadFlag()会调用函数MapPoint::EraseObservation()删除地图点对关键帧的观测,若地图点对关键帧的观测少于2,则地图点无法被三角化,就删除该地图点.
• 替换MapPoint的时机:
  1. LoopClosing线程中闭环矫正(LoopClosing::CorrectLoop())时当前关键帧和闭环关键帧上的地图点发生冲突时,会使用闭环关键帧的地图点替换当前关键帧的地图点.
  2. LoopClosing线程中闭环矫正函数LoopClosing::CorrectLoop()会调用LoopClosing::SearchAndFuse()将闭环关键帧的共视关键帧组中所有地图点投影到当前关键帧的共视关键帧组中,发生冲突时就会替换.

4. ORB-SLAM2代码详解04_帧Frame

4.1 各成员函数/变量

4.1.1 相机相关信息

Frame类与相机相关的参数大部分设为static类型,整个系统内的所有Frame对象共享同一份相机参数.

成员函数/变量	访问控制	意义
mbInitialComputations	public static	是否需要为Frame类的相机参数赋值 初始化为false,第一次为相机参数赋值后变为false
float fx, float fy float cx, float cy float invfx, float invfy	public static	相机内参
cv::Mat mK	public	相机内参矩阵 设为static是否更好?
float mb	public	相机基线,相机双目间的距离
float mbf	public	相机基线与焦距的乘积

这些参数首先由Tracking对象从配置文件TUM1.yaml内读入,再传给Frame类的构造函数,第一次调用Frame的构造函数时为这些成员变量赋值.

Tracking::Tracking(const string &strSettingPath, ...) {
​
    // 从配置文件中读取相机参数并构造内参矩阵
    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"];
​
    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);
​
    // ...
}
​
// 每传来一帧图像,就调用一次该函数
cv::Mat Tracking::GrabImageStereo(..., const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double &timestamp) {
    mCurrentFrame = Frame(mImGray, mK, mDistCoef, mbf, mThDepth);
​
    Track();
​
    // ...
}
​
// Frame构造函数
Frame::Frame(cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
    : mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) {
    
    // ...
        
    // 第一次调用Frame()构造函数时为所有static变量赋值
    if (mbInitialComputations) {
        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;      // 赋值完毕后将mbInitialComputations复位
    }
​
    mb = mbf / fx;
}

4.2 特征点提取

在Frame类构造函数中调用成员变量mpORBextractorLeft和mpORBextractorRight的()运算符进行特征点提取.

成员函数/变量	访问控制	意义
ORBextractor* mpORBextractorLeft ORBextractor* mpORBextractorRight	public	左右目图像的特征点提取器
cv::Mat mDescriptors cv::Mat mDescriptorsRight	public	左右目图像特征点描述子
std::vector mvKeys std::vector mvKeysRight	public	畸变矫正前的左/右目特征点
std::vector mvKeysUn	public	畸变矫正后的左目特征点
std::vector mvuRight	public	左目特征点在右目中匹配特征点的横坐标 (左右目匹配特征点的纵坐标相同)
std::vector mvDepth	public	特征点深度
float mThDepth	public	判断单目特征点和双目特征点的阈值 深度低于该值得特征点被认为是双目特征点 深度低于该值得特征点被认为是单目特征点

mvKeys、 mvKeysUn、 mvuRight、 mvDepth的坐标索引是对应的,也就是说对于第i个图像特征点:

• 其畸变矫正前的左目特征点是mvKeys[i].
• 其畸变矫正后的左目特征点是mvKeysUn[i].
• 其在右目图片中对应特征点的横坐标为mvuRight[i],纵坐标与mvKeys[i]的纵坐标相同.
• 特征点的深度是mvDepth[i].

对于单目特征点(单目相机输入的特征点或没有找到右目匹配的左目图像特征点),其mvuRight和mvDepth均为-1.

###4.2.1 特征点提取: ExtractORB()

成员函数/变量	访问控制	意义
void ExtractORB(int flag, const cv::Mat &im)	public	进行ORB特征提取

void Frame::ExtractORB(int flag, const cv::Mat &im) {
    if (flag == 0)      // flag==0, 表示对左图提取ORB特征点
        (*mpORBextractorLeft)(im, cv::Mat(), mvKeys, mDescriptors);
    else                // flag==1, 表示对右图提取ORB特征点
        (*mpORBextractorRight)(im, cv::Mat(), mvKeysRight, mDescriptorsRight);
}

4.3 ORB-SLAM2对双目/RGBD特征点的预处理

双目/RGBD相机中可以得到特征点的立体信息,包括右目特征点信息(mvuRight)、特征点深度信息(mvDepth)

• 对于双目相机,通过双目特征点匹配关系计算特征点的深度值.
• 对于RGBD相机,根据特征点深度构造虚拟的右目图像特征点.

成员函数/变量	访问控制	意义
void ComputeStereoMatches()	public	双目图像特征点匹配,用于双目相机输入图像预处理
void ComputeStereoFromRGBD(const cv::Mat &imDepth)	public	根据深度信息构造虚拟右目图像,用于RGBD相机输入图像预处理
cv::Mat UnprojectStereo(const int &i)	public	根据深度信息将第i个特征点反投影成MapPoint

通过这种预处理,在后面SLAM系统的其他部分中不再区分双目特征点和RGBD特征点,它们以双目特征点的形式被处理.(仅通过判断mvuRight[idx]判断某特征点是否有深度).

int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint *> &vpMapPoints, const float th) {
    // ...
    for (size_t idx : vIndices) {
        if (F.mvuRight[idx] > 0) {      // 通过判断 mvuRight[idx] 判断该特征点是否有深度
            // 针对有深度的特征点特殊处理
        } else {
            // 针对单目特征点的特殊处理
        }
    }
    // ...
}

4.4 双目视差公式

观测距离,基线,焦距,视差,根据三角形相似性:
$$\frac{z}{b}=\frac{z-f}{b-d}$$
得到:
$$d=\frac{z.f}{z}$$

4.5 双目特征点的处理:双目图像特征点匹配: ComputeStereoMatches()

双目相机分别提取到左右目特征点后对特征点进行双目匹配,并通过双目视差估计特征点深度.双目特征点匹配步骤:

1. 粗匹配: 根据特征点描述子距离和金字塔层级判断匹配.粗匹配关系是按行寻找的,对于左目图像中每个特征点,在右目图像对应行上寻找匹配特征点.
2. 精匹配: 根据特征点周围窗口内容相似度判断匹配.
3. 亚像素插值: 将特征点相似度与匹配坐标之间拟合成二次曲线,寻找最佳匹配位置(得到的是一个小数).
4. 记录右目匹配mvuRight和深度mvDepth信息.
5. 离群点筛选: 以平均相似度的2.1倍为标准,筛选离群点.

void Frame::ComputeStereoMatches() {
​
    mvuRight = vector<float>(N, -1.0f);
    mvDepth = vector<float>(N, -1.0f);
    
    // step0. 右目图像特征点逐行统计: 将右目图像中每个特征点注册到附近几行上
    vector<vector<size_t> > vRowIndices(nRows, vector<size_t>());   // 图像每行的1右目特征点索引
    for (int iR = 0; iR < mvKeysRight.size(); iR++) {
        const cv::KeyPoint &kp = mvKeysRight[iR];
        const float &kpY = kp.pt.y;
        const int maxr = ceil(kpY + 2.0f * mvScaleFactors[mvKeysRight[iR].octave]);
        const int minr = floor(kpY - 2.0f * mvScaleFactors[mvKeysRight[iR].octave]);
        for (int yi = minr; yi <= maxr; yi++)
            vRowIndices[yi].push_back(iR);
    }
​
    // step1. + 2. 粗匹配+精匹配
    const float minZ = mb, minD = 0, maxD = mbf / minZ;     // 根据视差公式计算两个特征点匹配搜索的范围
    const int thOrbDist = (ORBmatcher::TH_HIGH + ORBmatcher::TH_LOW) / 2;
​
    vector<pair<int, int> > vDistIdx;       // 保存特征点匹配
​
    for (int iL = 0; iL < N; iL++) {
​
        const cv::KeyPoint &kpL = mvKeys[iL];
        const int &levelL = kpL.octave;
        const float &vL = kpL.pt.y, &uL = kpL.pt.x;
​
        const vector<size_t> &vCandidates = vRowIndices[vL];
        if (vCandidates.empty()) continue;
        
        // step1. 粗匹配,根据特征点描述子和金字塔层级进行粗匹配
        int bestDist = ORBmatcher::TH_HIGH;
        size_t bestIdxR = 0;
        const cv::Mat &dL = mDescriptors.row(iL);
        for (size_t iC = 0; iC < vCandidates.size(); iC++) {
​
            const size_t iR = vCandidates[iC];
            const cv::KeyPoint &kpR = mvKeysRight[iR];
            if (kpR.octave < levelL - 1 || kpR.octave > levelL + 1)
                continue;
            const float &uR = kpR.pt.x;
​
            if (uR >= minU && uR <= maxU) {
                const cv::Mat &dR = mDescriptorsRight.row(iR);
                const int dist = ORBmatcher::DescriptorDistance(dL, dR);
                if (dist < bestDist) {
                    bestDist = dist;
                    bestIdxR = iR;
                }
            }
        }
​
        // step2. 精匹配: 滑动窗口匹配,根据匹配点周围5✖5窗口寻找精确匹配
        if (bestDist < thOrbDist) {
            const float uR0 = mvKeysRight[bestIdxR].pt.x;
            const float scaleFactor = mvInvScaleFactors[kpL.octave];
            const float scaleduL = round(kpL.pt.x * scaleFactor);
            const float scaledvL = round(kpL.pt.y * scaleFactor);
            const float scaleduR0 = round(uR0 * scaleFactor);
            const int w = 5;
            cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL - w, scaledvL + w + 1).colRange(scaleduL - w, scaleduL + w + 1);
            IL.convertTo(IL, CV_32F);
            IL = IL - IL.at<float>(w, w) * cv::Mat::ones(IL.rows, IL.cols, CV_32F);
            int bestDist = INT_MAX;
            int bestincR = 0;
            const int L = 5;
            vector<float> vDists;
            vDists.resize(2 * L + 1);
            const float iniu = scaleduR0 + L - w;
            const float endu = scaleduR0 + L + w + 1;
            for (int incR = -L; incR <= +L; incR++) {
                cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL - w, scaledvL + w + 1).colRange(scaleduR0 + incR - w, scaleduR0 + incR + w + 1);
                IR.convertTo(IR, CV_32F);
                IR = IR - IR.at<float>(w, w) * cv::Mat::ones(IR.rows, IR.cols, CV_32F);
                float dist = cv::norm(IL, IR, cv::NORM_L1);
                if (dist < bestDist) {
                    bestDist = dist;
                    bestincR = incR;
                }
                vDists[L + incR] = dist;
            }
            
            // step3. 亚像素插值: 将特征点匹配距离拟合成二次曲线,寻找二次曲线最低点(是一个小数)作为最优匹配点坐标
            const float dist1 = vDists[L + bestincR - 1];
            const float dist2 = vDists[L + bestincR];
            const float dist3 = vDists[L + bestincR + 1];
            const float deltaR = (dist1 - dist3) / (2.0f * (dist1 + dist3 - 2.0f * dist2));
​
            // step4. 记录特征点的右目和深度信息
            float bestuR = mvScaleFactors[kpL.octave] * ((float) scaleduR0 + (float) bestincR + deltaR);
            float disparity = (uL - bestuR);
            if (disparity >= minD && disparity < maxD) {
                mvDepth[iL] = mbf / disparity;
                mvuRight[iL] = bestuR;
                vDistIdx.push_back(pair<int, int>(bestDist, iL));
            }
        }
    }
    
    // step5. 删除离群点: 匹配距离大于平均匹配距离2.1倍的视为误匹配
    sort(vDistIdx.begin(), vDistIdx.end());
    const float median = vDistIdx[vDistIdx.size() / 2].first;
    const float thDist = 1.5f * 1.4f * median;
    for (int i = vDistIdx.size() - 1; i >= 0; i--) {
        if (vDistIdx[i].first < thDist)
            break;
        else {
            mvuRight[vDistIdx[i].second] = -1;
            mvDepth[vDistIdx[i].second] = -1;
        }
    }
}

4.6 RBGD特征点的处理: 根据深度信息构造虚拟右目图像: ComputeStereoFromRGBD()

对于RGB特征点,根据深度信息构造虚拟右目图像

void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth) {
    // 初始化 右目 和 深度 信息
    mvuRight = vector<float>(N, -1);
    mvDepth = vector<float>(N, -1);
​
    for (int i = 0; i < N; i++) {
        const cv::KeyPoint &kp = mvKeys[i];
        const cv::KeyPoint &kpU = mvKeysUn[i];
 
        // 从未畸变矫正的深度图中获取深度信息,从校正过后的左图中获取特征点位置信息,构造虚拟右目
        const float d = imDepth.at<float>(kp.pt.y, kp.pt.x);
        if (d > 0) {
            mvDepth[i] = d;
            mvuRight[i] = kpU.pt.x - mbf / d;
        }
    }
}

4.7 畸变矫正: UndistortKeyPoints()

成员函数/变量	访问控制	意义
cv::Mat mDistCoef	public	相机的畸变矫正参数
std::vector mvKeys std::vector mvKeysRight	public	畸变矫正前的左/右目特征点
std::vector mvKeysUn	public	畸变矫正后的左目特征点
void UndistortKeyPoints()	private	对所有特征点进行畸变矫正
float mnMinX float mnMaxX float mnMinY float mnMaxY	public	畸变矫正后的图像边界
void ComputeImageBounds(const cv::Mat &imLeft)	private	计算畸变矫正后的图像边界

实际上,畸变矫正只对单目和RGBD相机输入图像有效,双目相机的畸变矫正参数均为0,因为双目相机数据集在发布之前预先做了双目矫正.

• RGBD相机输入配置文件TUM1.yaml

  Camera.k1: 0.262383
  Camera.k2: -0.953104
  Camera.p1: -0.005358
  Camera.p2: 0.002628
  Camera.k3: 1.163314
  ​
  #....
  

• 双目相机输入配置文件EuRoC.yaml

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
​
# ...

双目矫正效果如下,双目矫正将两个相机的成像平面矫正到同一平面上.双目矫正之后两个相机的极线相互平行,极点在无穷远处,这也是我们在函数ComputeStereoMatches()中做极线搜索的理论基础.

---

UndistortKeyPoints()函数和ComputeImageBounds()内调用了cv::undistortPoints()函数对特征点进行畸变矫正

void Frame::UndistortKeyPoints() {
    // step1. 若输入图像是双目图像,则已做好了双目矫正,其畸变参数为0
    if (mDistCoef.at<float>(0) == 0.0) {
        mvKeysUn = mvKeys;
        return;
    }
​
    // 将特征点坐标转为undistortPoints()函数要求的格式
    cv::Mat mat(N, 2, CV_32F);
    for (int i = 0; i < N; i++) {
        mat.at<float>(i, 0) = mvKeys[i].pt.x;
        mat.at<float>(i, 1) = mvKeys[i].pt.y;
    }
    mat = mat.reshape(2);
    // 进行畸变矫正
    cv::undistortPoints(mat, mat, mK, mDistCoef, cv::Mat(), mK);
​
    // 记录校正后的特征点
    mat = mat.reshape(1);
    mvKeysUn.resize(N);
    for (int i = 0; i < N; i++) {
        cv::KeyPoint kp = mvKeys[i];
        mvKeysUn[i].pt.x = mat.at<float>(i, 0);
        mvKeysUn[i].pt.y = mat.at<float>(i, 1);
    }
}
​
// 通过计算图片顶点畸变矫正后的坐标来计算畸变矫正后的图片有效范围
void Frame::ComputeImageBounds(const cv::Mat &imLeft) {
    if (mDistCoef.at<float>(0) != 0.0) {
        // 4个顶点坐标
        cv::Mat mat(4, 2, CV_32F);
        mat.at<float>(0, 0) = 0.0;         //左上
        mat.at<float>(0, 1) = 0.0;
        mat.at<float>(1, 0) = imLeft.cols; //右上
        mat.at<float>(1, 1) = 0.0;
        mat.at<float>(2, 0) = 0.0;         //左下
        mat.at<float>(2, 1) = imLeft.rows;
        mat.at<float>(3, 0) = imLeft.cols; //右下
        mat.at<float>(3, 1) = imLeft.rows;
        // 畸变矫正
        mat = mat.reshape(2);
        cv::undistortPoints(mat, mat, mK, mDistCoef, cv::Mat(), mK);
        mat = mat.reshape(1);
        // 记录图片有效范围
        mnMinX = min(mat.at<float>(0, 0), mat.at<float>(2, 0));     //左上和左下横坐标最小的
        mnMaxX = max(mat.at<float>(1, 0), mat.at<float>(3, 0));     //右上和右下横坐标最大的
        mnMinY = min(mat.at<float>(0, 1), mat.at<float>(1, 1));     //左上和右上纵坐标最小的
        mnMaxY = max(mat.at<float>(2, 1), mat.at<float>(3, 1));     //左下和右下纵坐标最小的
    } else {
        mnMinX = 0.0f;
        mnMaxX = imLeft.cols;
        mnMinY = 0.0f;
        mnMaxY = imLeft.rows;
    }
}

4.8 特征点分配: AssignFeaturesToGrid()

在对特征点进行预处理后,将特征点分配到48行64列的网格中以加速匹配

成员函数/变量	访问控制	意义
FRAME_GRID_ROWS=48 FRAME_GRID_COLS=64	#DEFINE宏	网格行数/列数
float mfGridElementWidthInv float mfGridElementHeightInv	public static public static	每个网格的宽度/高度
std::vector mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]	public	每个网格内特征点编号列表
void AssignFeaturesToGrid()	private	将特征点分配到网格中
vector GetFeaturesInArea(float &x, float &y, float &r, int minLevel, int maxLevel)	public	获取半径为r的圆域内的特征点编号列表

成员变量std::vector mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]是一个二维数组,数组中每个元素是对应网格的所有特征点索引列表.

static成员变量mfGridElementWidthInv、mfGridElementHeightInv表示网格宽度/高度,它们在第一次调用Frame构造函数时被计算赋值.

// Frame构造函数
Frame::Frame(cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
    : mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth) {
    
    // ...
    
    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);
        
    // 第一次调用Frame()构造函数时为所有static变量赋值
    if (mbInitialComputations) {
        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;      // 赋值完毕后将mbInitialComputations复位
    }
​
    mb = mbf / fx;
}

函数AssignFeaturesToGrid()将特征点分配到网格中

void Frame::AssignFeaturesToGrid() {
    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);
    }
}

函数vector GetFeaturesInArea(float &x, float &y, float &r, int minLevel, int maxLevel)获取点(y,x)周围半径为r的圆域内所有特征点编号.

4.9 构造函数: Frame()

Frame()构造函数依次进行上面介绍的步骤:

// 双目相机Frame构造函数
Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor *extractorLeft, ORBextractor *extractorRight, ORBVocabulary *voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
    : mpORBvocabulary(voc), mpORBextractorLeft(extractorLeft), mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth), mpReferenceKF(static_cast<KeyFrame *>(NULL)) {
    
        
    // step0. 帧ID自增
    mnId = nNextId++;
​
    // step1. 计算金字塔参数
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor = log(mfScaleFactor);
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
​
    // step2. 提取双目图像特征点
    thread threadLeft(&Frame::ExtractORB, this, 0, imLeft);
    thread threadRight(&Frame::ExtractORB, this, 1, imRight);
    threadLeft.join();
    threadRight.join();
​
    N = mvKeys.size();
    if (mvKeys.empty())
        return;
​
    // step3. 畸变矫正,实际上UndistortKeyPoints()不对双目图像进行矫正
    UndistortKeyPoints();
​
    // step4. 双目图像特征点匹配
    ComputeStereoMatches();
        
    // step5. 第一次调用构造函数时计算static变量
    if (mbInitialComputations) {
        ComputeImageBounds(imLeft);
        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;
    }
    
    mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));    // 初始化本帧的地图点
    mvbOutlier = vector<bool>(N, false);    // 标记当前帧的地图点不是外点
    mb = mbf / fx;      // 计算双目基线长度
​
    // step6. 将特征点分配到网格中
    AssignFeaturesToGrid();
}

// RGBD相机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) {
    // step0. 帧ID自增
    mnId = nNextId++;
​
    // step1. 计算金字塔参数
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor = log(mfScaleFactor);
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
​
    // step2. 提取左目图像特征点
    ExtractORB(0, imGray);
​
    N = mvKeys.size();
    if (mvKeys.empty())
        return;
​
    // step3. 畸变矫正
    UndistortKeyPoints();
    
    // step4. 根据深度信息构造虚拟右目图像
    ComputeStereoFromRGBD(imDepth);
​
    mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));
    mvbOutlier = vector<bool>(N, false);
​
    // step5. 第一次调用构造函数时计算static变量
    if (mbInitialComputations) {
        ComputeImageBounds(imLeft);
        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;
    }
        
    mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));    // 初始化本帧的地图点
    mvbOutlier = vector<bool>(N, false);    // 标记当前帧的地图点不是外点
    mb = mbf / fx;      // 计算双目基线长度
​
​
    // step6. 将特征点分配到网格中
    AssignFeaturesToGrid();
}

4.10 Frame类的用途

Tracking类有两个Frame类型的成员变量

成员函数/变量	访问控制	意义
Frame mCurrentFrame	public	当前正在处理的帧
Frame mLastFrame	protected	上一帧

Tracking线程每收到一帧图像,就调用函数Tracking::GrabImageMonocular()、Tracking::GrabImageStereo()或Tracking::GrabImageRGBD()创建一个Frame对象,赋值给mCurrentFrame.

// 每传来一帧图像,就调用一次这个函数
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);
    }
​
    // 构造Frame
    if (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET) //没有成功初始化的前一个状态就是NO_IMAGES_YET
        mCurrentFrame = Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);
    else
        mCurrentFrame = Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth);
​
    // 跟踪
    Track();
​
    // 返回当前帧的位姿
    return mCurrentFrame.mTcw.clone();
}

Track()函数跟踪结束后,会将mCurrentFrame赋值给mLastFrame

void Tracking::Track() {
    
    // 进行跟踪
    // ...
    
    // 将当前帧记录为上一帧
    mLastFrame = Frame(mCurrentFrame);
    
    // ...
}

除了少数被选为KeyFrame的帧以外,大部分Frame对象的作用仅在于Tracking线程内追踪当前帧位姿,不会对LocalMapping线程和LoopClosing线程产生任何影响,在mLastFrame和mCurrentFrame更新之后就被系统销毁了.

5. ORB-SLAM2代码详解05_关键帧KeyFrame

5.1 各成员函数/变量

5.1.1 共视图: mConnectedKeyFrameWeights

能看到同一地图点的两关键帧之间存在共视关系,共视地图点的数量被称为权重.

成员函数/变量	访问控制	意义
std::map mConnectedKeyFrameWeights	protected	当前关键帧的共视关键帧及权重
std::vector mvpOrderedConnectedKeyFrames	protected	所有共视关键帧,按权重从大到小排序
std::vector mvOrderedWeights	protected	所有共视权重,按从大到小排序
void UpdateConnections()	public	基于当前关键帧对地图点的观测构造共视图
void AddConnection(KeyFrame* pKF, int &weight)	public 应为private	添加共视关键帧
void EraseConnection(KeyFrame* pKF)	public 应为private	删除共视关键帧
void UpdateBestCovisibles()	public 应为private	基于共视图信息修改对应变量
std::set GetConnectedKeyFrames()	public	get方法
std::vector GetVectorCovisibleKeyFrames()	public	get方法
std::vector GetBestCovisibilityKeyFrames(int &N)	public	get方法
std::vector GetCovisiblesByWeight(int &w)	public	get方法
int GetWeight(KeyFrame* pKF)	public	get方法

共视图结构由3个成员变量维护:

• mConnectedKeyFrameWeights是一个std::map,无序地保存当前关键帧的共视关键帧及权重.
• mvpOrderedConnectedKeyFrames和mvOrderedWeights按权重降序分别保存当前关键帧的共视关键帧列表和权重列表.

5.1.2 基于对地图点的观测重新构造共视图: UpdateConnections()

这3个变量由函数KeyFrame::UpdateConnections()进行初始化和维护,基于当前关键帧看到的地图点信息重新生成共视关键帧.

yFrame*> GetBestCovisibilityKeyFrames(int &N)	public	get方法
std::vector GetCovisiblesByWeight(int &w)	public	get方法
int GetWeight(KeyFrame* pKF)	public	get方法

共视图结构由3个成员变量维护:

• mConnectedKeyFrameWeights是一个std::map,无序地保存当前关键帧的共视关键帧及权重.
• mvpOrderedConnectedKeyFrames和mvOrderedWeights按权重降序分别保存当前关键帧的共视关键帧列表和权重列表.

5.1.3 基于对地图点的观测重新构造共视图: UpdateConnections()

这3个变量由函数KeyFrame::UpdateConnections()进行初始化和维护,基于当前关键帧看到的地图点信息重新生成共视关键帧.

void KeyFrame::UpdateConnections() {
    
    // 1. 通过遍历当前帧地图点获取其与其它关键帧的共视程度,存入变量KFcounter中
    vector<MapPoint *> vpMP;
    {
        unique_lock<mutex> lockMPs(mMutexFeatures);
        vpMP = mvpMapPoints;
    }
    map<KeyFrame *, int> KFcounter; 
    for (MapPoint *pMP : vpMP) {
        map<KeyFrame *, size_t> observations = pMP->GetObservations();
        for (map<KeyFrame *, size_t>::iterator mit = observations.begin(); mit != observations.end(); mit++) {
            if (mit->first->mnId == mnId)       // 与当前关键帧本身不算共视
                continue;
            KFcounter[mit->first]++;
        }
    }
  
    // step2. 找到与当前关键帧共视程度超过15的关键帧,存入变量vPairs中
    vector<pair<int, KeyFrame *> > vPairs;
    int th = 15;
    int nmax = 0;
    KeyFrame *pKFmax = NULL;   
    for (map<KeyFrame *, int>::iterator mit = KFcounter.begin(), mend = KFcounter.end(); mit != mend; mit++) {
        if (mit->second > nmax) {
            nmax = mit->second;
            pKFmax = mit->first;
        }
        if (mit->second >= th) {
            vPairs.push_back(make_pair(mit->second, mit->first));
            (mit->first)->AddConnection(this, mit->second);             // 对超过阈值的共视边建立连接
        }
    }
​
    //  step3. 对关键帧按照共视权重降序排序,存入变量mvpOrderedConnectedKeyFrames和mvOrderedWeights中
    sort(vPairs.begin(), vPairs.end());
    list<KeyFrame *> lKFs;
    list<int> lWs;
    for (size_t i = 0; i < vPairs.size(); i++) {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }
    {
        unique_lock<mutex> lockCon(mMutexConnections);
        mConnectedKeyFrameWeights = KFcounter;
        mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
​
        // step4. 对于第一次加入生成树的关键帧,取共视程度最高的关键帧为父关键帧
        if (mbFirstConnection && mnId != 0) {
            mpParent = mvpOrderedConnectedKeyFrames.front();
            mpParent->AddChild(this);
            mbFirstConnection = false;
        }
    }
}

只要关键帧与地图点间的连接关系发生变化(包括关键帧创建和地图点重新匹配关键帧特征点),函数KeyFrame::UpdateConnections()就会被调用.具体来说,函数KeyFrame::UpdateConnections()的调用时机包括:

• Tracking线程中初始化函数Tracking::StereoInitialization()或Tracking::MonocularInitialization()函数创建关键帧后会调用KeyFrame::UpdateConnections()初始化共视图信息.
• LocalMapping线程接受到新关键帧时会调用函数LocalMapping::ProcessNewKeyFrame()处理跟踪过程中加入的地图点,之后会调用KeyFrame::UpdateConnections()初始化共视图信息.(实际上这里处理的是Tracking线程中函数Tracking::CreateNewKeyFrame()创建的关键帧)
• LocalMapping线程处理完毕缓冲队列内所有关键帧后会调用LocalMapping::SearchInNeighbors()融合当前关键帧和共视关键帧间的重复地图点,之后会调用KeyFrame::UpdateConnections()更新共视图信息.
• LoopClosing线程闭环矫正函数LoopClosing::CorrectLoop()会多次调用KeyFrame::UpdateConnections()更新共视图信息.

---

函数AddConnection(KeyFrame* pKF, const int &weight)和EraseConnection(KeyFrame* pKF)先对变量mConnectedKeyFrameWeights进行修改,再调用函数UpdateBestCovisibles()修改变量mvpOrderedConnectedKeyFrames和mvOrderedWeights.

这3个函数都只在函数KeyFrame::UpdateConnections()内部被调用了,应该设为私有成员函数.

void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight) {
    // step1. 修改变量mConnectedKeyFrameWeights
    {
        unique_lock<mutex> lock(mMutexConnections);
​
        if (!mConnectedKeyFrameWeights.count(pKF) || mConnectedKeyFrameWeights[pKF] != weight)
            mConnectedKeyFrameWeights[pKF] = weight;
        else
            return;
    }
    
    // step2. 调用函数UpdateBestCovisibles()修改变量mvpOrderedConnectedKeyFrames和mvOrderedWeights
    UpdateBestCovisibles();
}
​
​
void KeyFrame::EraseConnection(KeyFrame *pKF) {
    // step1. 修改变量mConnectedKeyFrameWeights
    bool bUpdate = false;
    {
        unique_lock<mutex> lock(mMutexConnections);
        if (mConnectedKeyFrameWeights.count(pKF)) {
            mConnectedKeyFrameWeights.erase(pKF);
            bUpdate = true;
        }
    }
​
    // step2. 调用函数UpdateBestCovisibles()修改变量mvpOrderedConnectedKeyFrames和mvOrderedWeights
    if (bUpdate)
        UpdateBestCovisibles();
}
​
void KeyFrame::UpdateBestCovisibles() {    
    unique_lock<mutex> lock(mMutexConnections);
    
    // 取出所有关键帧进行排序,排序结果存入变量mvpOrderedConnectedKeyFrames和mvOrderedWeights中
    vector<pair<int, KeyFrame *> > vPairs;
    vPairs.reserve(mConnectedKeyFrameWeights.size());
    for (map<KeyFrame *, int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend = mConnectedKeyFrameWeights.end(); mit != mend; mit++)
        vPairs.push_back(make_pair(mit->second, mit->first));
​
    sort(vPairs.begin(), vPairs.end());
    list<KeyFrame *> lKFs; 
    list<int> lWs; 
    for (size_t i = 0, iend = vPairs.size(); i < iend; i++) {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }
​
    mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());
    mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
}

5.2 生成树: mpParent、mspChildrens

生成树是一种稀疏连接,以最小的边数保存图中所有节点.对于含有N个节点的图,只需构造一个N-1条边的最小生成树就可以将所有节点连接起来.

下图表示含有一个10个节点,20条边的稠密图;粗黑线代表其最小生成树,只需9条边即可将所有节点连接起来.

在ORB-SLAM2中,保存所有关键帧构成的最小生成树(优先选择权重大的边作为生成树的边),在回环闭合时只需对最小生成树做BA优化就能以最小代价优化所有关键帧和地图点的位姿,相比于优化共视图大大减少了计算量.(实际上并没有对最小生成树做BA优化,而是对包含生成树的本质图做BA优化)

成员函数/变量	访问控制	意义
bool mbFirstConnection	protected	当前关键帧是否还未加入到生成树 构造函数中初始化为true,加入生成树后置为false
KeyFrame* mpParent	protected	当前关键帧在生成树中的父节点
std::set mspChildrens	protected	当前关键帧在生成树中的子节点列表
KeyFrame* GetParent()	public	mpParent的get方法
void ChangeParent(KeyFrame* pKF)	public 应为private	mpParent的set方法
std::set GetChilds()	public	mspChildrens的get方法
void AddChild(KeyFrame* pKF)	public 应为private	添加子节点,mspChildrens的set方法
void EraseChild(KeyFrame* pKF)	public 应为private	删除子节点,mspChildrens的set方法
bool hasChild(KeyFrame* pKF)	public	判断mspChildrens是否为空

生成树结构由成员变量mpParent和mspChildrens维护.我们主要关注生成树结构发生改变的时机.

• 关键帧增加到生成树中的时机:

  成功创建关键帧之后会调用函数KeyFrame::UpdateConnections(),该函数第一次被调用时会将该新关键帧加入到生成树中.

  新关键帧的父关键帧会被设为其共视程度最高的共视关键帧.

void KeyFrame::UpdateConnections() {
    
    // 更新共视图信息
    // ...
    
    // 更新关键帧信息: 对于第一次加入生成树的关键帧,取共视程度最高的关键帧为父关键帧
    // 该操作会改变当前关键帧的成员变量mpParent和父关键帧的成员变量mspChildrens
    unique_lock<mutex> lockCon(mMutexConnections);
    if (mbFirstConnection && mnId != 0) {
        mpParent = mvpOrderedConnectedKeyFrames.front();
        mpParent->AddChild(this);
        mbFirstConnection = false;
    }
}

• 共视图的改变(除了删除关键帧以外)不会引发生成树的改变.
• 只有当某个关键帧删除时,与其相连的生成树结构在会发生改变.(因为生成树是个单线联系的结构,没有冗余,一旦某关键帧删除了就得更新树结构才能保证所有关键帧依旧相连).生成树结构改变的方式类似于最小生成树算法中的加边法,见后文对函数setbadflag()的分析.

5.3 关键帧的删除

成员函数/变量	访问控制	意义	初值
bool mbBad	protected	标记是坏帧	false
bool isBad()	public	mbBad的get方法
void SetBadFlag()	public	真的执行删除
bool mbNotErase	protected	当前关键帧是否具有不被删除的特权	false
bool mbToBeErased	protected	当前关键帧是否曾被豁免过删除	false
void SetNotErase()	public	mbNotErase的set方法
void SetErase()	public

与MapPoint类似,函数KeyFrame::SetBadFlag()对KeyFrame的删除过程也采取先标记再清除的方式: 先将坏帧标记mBad置为true,再依次处理其各成员变量.

5.4 参与回环检测的关键帧具有不被删除的特权: mbNotErase

参与回环检测的关键帧具有不被删除的特权,该特权由成员变量mbNotErase存储,创建KeyFrame对象时该成员变量默认被初始化为false.

若某关键帧参与了回环检测,LoopClosing线程就会就调用函数KeyFrame::SetNotErase()将该关键帧的成员变量mbNotErase设为true,标记该关键帧暂时不要被删除.

void KeyFrame::SetNotErase() {
    unique_lock<mutex> lock(mMutexConnections);
    mbNotErase = true;
}

在删除函数SetBadFlag()起始先根据成员变量mbNotErase判断当前KeyFrame是否具有豁免删除的特权.若当前KeyFrame的mbNotErase为true,则函数SetBadFlag()不能删除当前KeyFrame,但会将其成员变量mbToBeErased置为true.

void KeyFrame::SetBadFlag() {
    // step1. 特殊情况:豁免 第一帧 和 具有mbNotErase特权的帧
    {
        unique_lock<mutex> lock(mMutexConnections);
​
        if (mnId == 0)
            return;
        else if (mbNotErase) {
            mbToBeErased = true;
            return;
        }
    }
    
    // 两步删除: 先逻辑删除,再物理删除...
}

成员变量mbToBeErased标记当前KeyFrame是否被豁免过删除特权.LoopClosing线程不再需要某关键帧时,会调用函数KeyFrame::SetErase()剥夺该关键帧不被删除的特权,将成员变量mbNotErase复位为false;同时检查成员变量mbToBeErased,若mbToBeErased为true就会调用函数KeyFrame::SetBadFlag()删除该关键帧.

void KeyFrame::SetErase() {
    {
        unique_lock<mutex> lock(mMutexConnections);
        // 若当前关键帧没参与回环检测,但其它帧与当前关键帧形成回环关系,也不应当删除当前关键帧
        if (mspLoopEdges.empty()) {
            mbNotErase = false;
        }
    }
​
    // mbToBeErased：删除之前记录的想要删但时机不合适没有删除的帧
    if (mbToBeErased) {
        SetBadFlag();
    }
}

5.5 删除关键帧时维护共视图和生成树

函数SetBadFlag()在删除关键帧的时维护其共视图和生成树结构.共视图结构的维护比较简单,这里主要关心如何维护生成树的结构.

当一个关键帧被删除时,其父关键帧和所有子关键帧的生成树信息也会受到影响,需要为其所有子关键帧寻找新的父关键帧,如果父关键帧找的不好的话,就会产生回环,导致生成树就断开.

被删除关键帧的子关键帧所有可能的父关键帧包括其兄弟关键帧和其被删除关键帧的父关键帧.以下图为例,关键帧4可能的父关键帧包括关键帧3、5、6和7.

采用类似于最小生成树算法中的加边法重新构建生成树结构: 每次循环取权重最高的候选边建立父子连接关系,并将新加入生成树的子节点到加入候选父节点集合sParentCandidates中.

void KeyFrame::SetBadFlag() {
    // step1. 特殊情况:豁免 第一帧 和 具有mbNotErase特权的帧
    {
        unique_lock<mutex> lock(mMutexConnections);
​
        if (mnId == 0)
            return;
        else if (mbNotErase) {
            mbToBeErased = true;
            return;
        }
    }
​
    // step2. 从共视关键帧的共视图中删除本关键帧
    for (auto mit : mConnectedKeyFrameWeights)
        mit.first->EraseConnection(this);
​
    // step3. 删除当前关键帧中地图点对本帧的观测
    for (size_t i = 0; i < mvpMapPoints.size(); i++)
        if (mvpMapPoints[i])
            mvpMapPoints[i]->EraseObservation(this);
​
    {
        // step4. 删除共视图
        unique_lock<mutex> lock(mMutexConnections);
        unique_lock<mutex> lock1(mMutexFeatures);
        mConnectedKeyFrameWeights.clear();
        mvpOrderedConnectedKeyFrames.clear();
​
        // step5. 更新生成树结构
        set<KeyFrame *> sParentCandidates;
        sParentCandidates.insert(mpParent);
​
        while (!mspChildrens.empty()) {
            bool bContinue = false;
            int max = -1;
            KeyFrame *pC;
            KeyFrame *pP;
            for (KeyFrame *pKF : mspChildrens) {
                if (pKF->isBad())
                    continue;
​
                vector<KeyFrame *> vpConnected = pKF->GetVectorCovisibleKeyFrames();
​
                for (size_t i = 0, iend = vpConnected.size(); i < iend; i++) {
                    for (set<KeyFrame *>::iterator spcit = sParentCandidates.begin(), spcend = sParentCandidates.end();
                         spcit != spcend; spcit++) {
                        if (vpConnected[i]->mnId == (*spcit)->mnId) {
                            int w = pKF->GetWeight(vpConnected[i]);
                            if (w > max) {
                                pC = pKF;                   
                                pP = vpConnected[i];        
                                max = w;                    
                                bContinue = true;           
                            }
                        }
                    }
                }
            }
​
            if (bContinue) {
                pC->ChangeParent(pP);
                sParentCandidates.insert(pC);
                mspChildrens.erase(pC);
            } else
                break;
        }
​
        if (!mspChildrens.empty())
            for (set<KeyFrame *>::iterator sit = mspChildrens.begin(); sit != mspChildrens.end(); sit++) {
                (*sit)->ChangeParent(mpParent);
            }
​
        mpParent->EraseChild(this);
        mTcp = Tcw * mpParent->GetPoseInverse();
        // step6. 将当前关键帧的 mbBad 置为 true
        mbBad = true;
    } 
    
    // step7. 从地图中删除当前关键帧
    mpMap->EraseKeyFrame(this);
    mpKeyFrameDB->erase(this);
}

5.6 对地图点的观测

KeyFrame类除了像一般的Frame类那样保存二维图像特征点以外,还保存三维地图点MapPoint信息.

关键帧观测到的地图点列表由成员变量mvpMapPoints保存,下面是一些对该成员变量进行增删改查的成员函数,就是简单的列表操作,没什么值得说的地方.

成员函数/变量	访问控制	意义
std::vector mvpMapPoints	protected	当前关键帧观测到的地图点列表
void AddMapPoint(MapPoint* pMP, const size_t &idx)	public
void EraseMapPointMatch(const size_t &idx)	public
void EraseMapPointMatch(MapPoint* pMP)	public
void ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP)	public
std::set GetMapPoints()	public
std::vector GetMapPointMatches()	public
int TrackedMapPoints(const int &minObs)	public
MapPoint* GetMapPoint(const size_t &idx)	public

值得关心的是上述函数的调用时机,也就是说参考帧何时与地图点发生关系:

• 关键帧增加对地图点观测的时机:

  1. Tracking线程和LocalMapping线程创建新地图点后,会马上调用函数KeyFrame::AddMapPoint()添加当前关键帧对该地图点的观测.
  2. LocalMapping线程处理完毕缓冲队列内所有关键帧后会调用LocalMapping::SearchInNeighbors()融合当前关键帧和共视关键帧间的重复地图点,其中调用函数ORBmatcher::Fuse()实现融合过程中会调用函数KeyFrame::AddMapPoint().
  3. LoopClosing线程闭环矫正函数LoopClosing::CorrectLoop()将闭环关键帧与其匹配关键帧间的地图进行融合,会调用函数KeyFrame::AddMapPoint().

• 关键帧替换和删除对地图点观测的时机:

  1. MapPoint删除函数MapPoint::SetBadFlag()或替换函数MapPoint::Replace()会调用KeyFrame::EraseMapPointMatch()和KeyFrame::ReplaceMapPointMatch()删除和替换关键针对地图点的观测.

  2. LocalMapping线程调用进行局部BA优化的函数Optimizer::LocalBundleAdjustment()内部调用函数KeyFrame::EraseMapPointMatch()删除对重投影误差较大的地图点的观测.

5.7 回环检测与本质图

成员函数/变量	访问控制	意义
std::set mspLoopEdge	protected	和当前帧形成回环的关键帧集合
set GetLoopEdges()	public	mspLoopEdge的get函数
void AddLoopEdge(KeyFrame *pKF)	public	mspLoopEdge的set函数

LoopClosing线程中回环矫正函数LoopClosing::CorrectLoop()在调用本质图BA优化函数Optimizer::OptimizeEssentialGraph()之前会调用函数KeyFrame::AddLoopEdge(),在当前关键帧和其闭环匹配关键帧间添加回环关系.

在调用本质图BA优化函数Optimizer::OptimizeEssentialGraph()中会调用函数KeyFrame::GetLoopEdges()将所有闭环关系加入到本质图中进行优化.

5.8 KeyFrame`的用途

KeyFrame类的生命周期

• KeyFrame的创建:

  Tracking线程中通过函数Tracking::NeedNewKeyFrame()判断是否需要关键帧,若需要关键帧,则调用函数Tracking::CreateNewKeyFrame()创建关键帧.

• KeyFrame的销毁:

  LocalMapping线程剔除冗余关键帧函数LocalMapping::KeyFrameCulling()中若检查到某关键帧为冗余关键帧,则调用函数KeyFrame::SetBadFlag()删除关键帧.

6. ORB-SLAM2代码详解06_单目初始化器Initializer

6.1 各成员变量/函数

Initializer类仅用于单目相机初始化,双目/RGBD相机初始化不用这个类.

成员变量名中: 1代表参考帧(reference frame)中特征点编号,2代表当前帧(current frame)中特征点编号.

各成员函数/变量	访问控制	意义
vector mvKeys1	private	参考帧(reference frame)中的特征点
vector mvKeys2	private	当前帧(current frame)中的特征点
vector> mvMatches12	private	从参考帧到当前帧的匹配特征点对
vector mvbMatched1	private	参考帧特征点是否在当前帧存在匹配特征点
cv::Mat mK	private	相机内参
float mSigma, mSigma2	private	重投影误差阈值及其平方
int mMaxIterations	private	RANSAC迭代次数
vector> mvSets	private	二维容器N✖8 每一层保存RANSAC计算H和F矩阵所需的八对点

6.1.1 初始化函数: Initialize()

主函数Initialize()根据两帧间的匹配关系恢复帧间运动并计算地图点位姿.

bool Initializer::Initialize(const Frame &CurrentFrame,
                             const vector<int> &vMatches12,
                             cv::Mat &R21, cv::Mat &t21,
                             vector<cv::Point3f> &vP3D,
                             vector<bool> &vbTriangulated) {
​
    // 初始化器Initializer对象创建时就已指定mvKeys1,调用本函数只需指定mvKeys2即可
    mvKeys2 = CurrentFrame.mvKeysUn;        // current frame中的特征点
    mvMatches12.reserve(mvKeys2.size());
    mvbMatched1.resize(mvKeys1.size());
​
    // step1. 将vMatches12拷贝到mvMatches12,mvMatches12只保存匹配上的特征点对
    for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) {
        if (vMatches12[i] >= 0) {
            mvMatches12.push_back(make_pair(i, vMatches12[i]));
            mvbMatched1[i] = true;
        } else
            mvbMatched1[i] = false;
    }
​
    // step2. 准备RANSAC运算中需要的特征点对
    const int N = mvMatches12.size();
    vector<size_t> vAllIndices;
    for (int i = 0; i < N; i++) {
        vAllIndices.push_back(i);
    }
    mvSets = vector<vector<size_t> >(mMaxIterations, vector<size_t>(8, 0));
    for (int it = 0; it < mMaxIterations; it++) {
        vector<size_t> vAvailableIndices = vAllIndices;
        for (size_t j = 0; j < 8; j++) {
            int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size() - 1);
            int idx = vAvailableIndices[randi];
            mvSets[it][j] = idx;
            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }
    }
​
    // step3. 计算F矩阵和H矩阵及其置信程度
    vector<bool> vbMatchesInliersH, vbMatchesInliersF;
    float SH, SF;
    cv::Mat H, F;
​
    thread threadH(&Initializer::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H));
    thread threadF(&Initializer::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F));
    threadH.join();
    threadF.join();
​
    // step4. 根据比分计算使用哪个矩阵恢复运动
    float RH = SH / (SH + SF); 
    if (RH > 0.40)
        return ReconstructH(vbMatchesInliersH, H, mK, R21, t21, vP3D, vbTriangulated, 1.0, 50);
    else
        return ReconstructF(vbMatchesInliersF, F, mK, R21, t21, vP3D, vbTriangulated, 1.0, 50);
}

6.2 计算基础矩阵F和单应矩阵H

6.2.1 RANSAC算法

少数外点会极大影响计算结果的准确度.随着采样数量的增加,外点数量也会同时增加,这是一种系统误差,无法通过增加采样点来解决.

RANSAC(Random sample consensus,随机采样一致性)算法的思路是少量多次重复实验,每次实验仅使用尽可能少的点来计算,并统计本次计算中的内点数.只要尝试次数足够多的话,总会找到一个包含所有内点的解.

RANSAC算法的核心是减少每次迭代所需的采样点数.从原理上来说,计算F矩阵最少只需要7对匹配点,计算H矩阵最少只需要4对匹配点;ORB-SLAM2中为了编程方便,每次迭代使用8对匹配点计算F和H.

6.2.2 计算基础矩阵F: FindFundamental()

设点P在相机1、2坐标系下的坐标分别为、,在相机$X_1,X_2$成像平面下的像素坐标分别为$x_1$、$x_2$,有:
$$\begin{gathered} {X_2}^{T}E{X}_1=0 \\ {x}_1=K_1{X}_1 \\ {x}_2=K_2{X}_2 \end{gathered}$$
其中本质矩阵$E=t^{\wedge }R$.
$${x_2}^T{k_2}^{T}E{k_1}^T{x}_1=0$$
令$F={k_2}^{T}E{k_1}^T$可得：
$${x_2}^{T}F{x}_1=0$$

void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21) {
​
    const int N = vbMatchesInliers.size();
​
    // step1. 特征点归一化
    vector<cv::Point2f> vPn1, vPn2;
    cv::Mat T1, T2;
    Normalize(mvKeys1, vPn1, T1);
    Normalize(mvKeys2, vPn2, T2);
    cv::Mat T2t = T2.t();       // 用于恢复原始尺度
​
    // step2. RANSAC循环
    score = 0.0;                                    // 最优解得分
    vbMatchesInliers = vector<bool>(N, false);      // 最优解对应的内点
    for (int it = 0; it < mMaxIterations; it++) {
        vector<cv::Point2f> vPn1i(8);
        vector<cv::Point2f> vPn2i(8);
        cv::Mat F21i;
        vector<bool> vbCurrentInliers(N, false);
        float currentScore;
​
        for (int j = 0; j < 8; j++) {
            int idx = mvSets[it][j];
            vPn1i[j] = vPn1[mvMatches12[idx].first];        // first存储在参考帧1中的特征点索引
            vPn2i[j] = vPn2[mvMatches12[idx].second];       // second存储在当前帧2中的特征点索引
        }
​
        // step3. 八点法计算单应矩阵H
        cv::Mat Fn = ComputeF21(vPn1i, vPn2i);
​
        // step4. 恢复原始尺度
        F21i = T2t * Fn * T1;
​
        // step5. 根据重投影误差进行卡方检验
        currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
​
        // step6. 记录最优解
        if (currentScore > score) {
            F21 = F21i.clone();
            vbMatchesInliers = vbCurrentInliers;
            score = currentScore;
        }
    }
}
​

6.2.3 八点法计算F矩阵: ComputeF21()

F矩阵的约束:

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

展开成:

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

由于F矩阵的尺度不变性,只需8对特征点即可提供足够的约束.

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

上图中矩阵是一个的矩阵,是一个的向量;上述方程是一个超定方程,使用SVD分解求最小二乘解.

cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) {
​
    const int N = vP1.size();
​
    // step1. 构造A矩阵
    cv::Mat A(N, 9, CV_32F);
    for (int i = 0; i < N; i++) {
        const float u1 = vP1[i].x;
        const float v1 = vP1[i].y;
        const float u2 = vP2[i].x;
        const float v2 = vP2[i].y;
        A.at<float>(i, 0) = u2 * u1;
        A.at<float>(i, 1) = u2 * v1;
        A.at<float>(i, 2) = u2;
        A.at<float>(i, 3) = v2 * u1;
        A.at<float>(i, 4) = v2 * v1;
        A.at<float>(i, 5) = v2;
        A.at<float>(i, 6) = u1;
        A.at<float>(i, 7) = v1;
        A.at<float>(i, 8) = 1;
    }
​
    // step2. 奇异值分解,取vt最后一行
    cv::Mat u, w, vt;
    cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最后一列
​
    // step3. 将F矩阵的秩强制置为2
    cv::SVDecomp(Fpre, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    w.at<float>(2) = 0;
    return u * cv::Mat::diag(w) * vt;
}

6.2.4 计算单应矩阵H: FindHomography()

以下两种情况更适合使用单应矩阵进行初始化:

1. 相机看到的场景是一个平面.
2. 连续两帧间没发生平移,只发生旋转.

使用八点法求解单应矩阵H的原理类似:

正常来说只用4对匹配点就可以计算单应矩阵H,但ORB-SLAM2每次RANSAC迭代取8对匹配点来计算H.个人理解这是为了和八点法计算基础矩阵H相对应,都使用8对匹配点来计算,便于后面比较分数优劣.

void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21) {
​
    const int N = mvMatches12.size();
​
    // step1. 特征点归一化
    vector<cv::Point2f> vPn1, vPn2;
    cv::Mat T1, T2;
    Normalize(mvKeys1, vPn1, T1);
    Normalize(mvKeys2, vPn2, T2);
    cv::Mat T2inv = T2.inv();       // 用于恢复原始尺度
​
    // step2. RANSAC循环
    score = 0.0;                                    // 最优解得分
    vbMatchesInliers = vector<bool>(N, false);      // 最优解对应的内点
    for (int it = 0; it < mMaxIterations; it++) {
        vector<cv::Point2f> vPn1i(8);               
        vector<cv::Point2f> vPn2i(8);
        cv::Mat H21i, H12i;
        vector<bool> vbCurrentInliers(N, false);
        float currentScore;
​
        for (size_t j = 0; j < 8; j++) {
            int idx = mvSets[it][j];
            vPn1i[j] = vPn1[mvMatches12[idx].first];    // first存储在参考帧1中的特征点索引
            vPn2i[j] = vPn2[mvMatches12[idx].second];   // second存储在当前帧2中的特征点索引
        }
​
        // step3. 八点法计算单应矩阵H
        cv::Mat Hn = ComputeH21(vPn1i, vPn2i);
​
        // step4. 恢复原始尺度
        H21i = T2inv * Hn * T1;
        H12i = H21i.inv();
​
        // step5. 根据重投影误差进行卡方检验
        currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
​
        // step6. 记录最优解
        if (currentScore > score) {
            H21 = H21i.clone();
            vbMatchesInliers = vbCurrentInliers;
            score = currentScore;
        }
    }
}

cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2) {
    
    const int N = vP1.size();
​
    // step1. 构造A矩阵
    cv::Mat A(2 * N, 9, CV_32F);
    for (int i = 0; i < N; i++) {
        const float u1 = vP1[i].x;
        const float v1 = vP1[i].y;
        const float u2 = vP2[i].x;
        const float v2 = vP2[i].y;
        A.at<float>(2 * i, 0) = 0.0;
        A.at<float>(2 * i, 1) = 0.0;
        A.at<float>(2 * i, 2) = 0.0;
        A.at<float>(2 * i, 3) = -u1;
        A.at<float>(2 * i, 4) = -v1;
        A.at<float>(2 * i, 5) = -1;
        A.at<float>(2 * i, 6) = v2 * u1;
        A.at<float>(2 * i, 7) = v2 * v1;
        A.at<float>(2 * i, 8) = v2;
        A.at<float>(2 * i + 1, 0) = u1;
        A.at<float>(2 * i + 1, 1) = v1;
        A.at<float>(2 * i + 1, 2) = 1;
        A.at<float>(2 * i + 1, 3) = 0.0;
        A.at<float>(2 * i + 1, 4) = 0.0;
        A.at<float>(2 * i + 1, 5) = 0.0;
        A.at<float>(2 * i + 1, 6) = -u2 * u1;
        A.at<float>(2 * i + 1, 7) = -u2 * v1;
        A.at<float>(2 * i + 1, 8) = -u2;
    }
​
    // step2. 奇异值分解,取vt最后一行
    cv::Mat u, w, vt;
    cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A |cv::SVD::FULL_UV); 
    return vt.row(8).reshape(0, 3);
}

6.2.5 卡方检验计算置信度得分: CheckFundamental()、CheckHomography()

卡方检验通过构造检验统计量来比较期望结果和实际结果之间的差别,从而得出观察频数极值的发生概率.

根据重投影误差构造统计量,其值越大,观察结果和期望结果之间的差别越显著,某次计算越可能用到了外点.

统计量置信度阈值与被检验变量自由度有关: 单目特征点重投影误差的自由度为2(u,v),双目特征点重投影误差自由度为3(u,v,ur).

取95%置信度下的卡方检验统计量阈值

• 若统计量大于该阈值,则认为计算矩阵使用到了外点,将其分数设为0.
• 若统计量小于该阈值,则将统计量裕量设为该解的置信度分数.

float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12,vector<bool> &vbMatchesInliers, float sigma) {
    const int N = mvMatches12.size();
    
    // 取出单应矩阵H各位上的值
    const float h11 = H21.at<float>(0, 0);
    const float h12 = H21.at<float>(0, 1);
    const float h13 = H21.at<float>(0, 2);
    const float h21 = H21.at<float>(1, 0);
    const float h22 = H21.at<float>(1, 1);
    const float h23 = H21.at<float>(1, 2);
    const float h31 = H21.at<float>(2, 0);
    const float h32 = H21.at<float>(2, 1);
    const float h33 = H21.at<float>(2, 2);
​
    const float h11inv = H12.at<float>(0, 0);
    const float h12inv = H12.at<float>(0, 1);
    const float h13inv = H12.at<float>(0, 2);
    const float h21inv = H12.at<float>(1, 0);
    const float h22inv = H12.at<float>(1, 1);
    const float h23inv = H12.at<float>(1, 2);
    const float h31inv = H12.at<float>(2, 0);
    const float h32inv = H12.at<float>(2, 1);
    const float h33inv = H12.at<float>(2, 2);
    
    vbMatchesInliers.resize(N);     // 标记是否是内点
    float score = 0;                // 置信度得分
    const float th = 5.991;         // 自由度为2,显著性水平为0.05的卡方分布对应的临界阈值
    const float invSigmaSquare = 1.0 / (sigma * sigma);     // 信息矩阵,方差平方的倒数
​
    
    // 双向投影,计算加权投影误差
    for (int i = 0; i < N; i++) {
        bool bIn = true;
​
        // step1. 提取特征点对
        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
        const float u1 = kp1.pt.x;
        const float v1 = kp1.pt.y;
        const float u2 = kp2.pt.x;
        const float v2 = kp2.pt.y;
​
        // step2. 计算img2到img1的重投影误差
        const float w2in1inv = 1.0 / (h31inv * u2 + h32inv * v2 + h33inv);
        const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv;
        const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv;
        const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);
        const float chiSquare1 = squareDist1 * invSigmaSquare;
​
        // step3. 离群点标记上,非离群点累加计算得分
        if (chiSquare1 > th)
            bIn = false;
        else
            score += th - chiSquare1;
​
        // step4. 计算img1到img2的重投影误差
        const float w1in2inv = 1.0 / (h31 * u1 + h32 * v1 + h33);
        const float u1in2 = (h11 * u1 + h12 * v1 + h13) * w1in2inv;
        const float v1in2 = (h21 * u1 + h22 * v1 + h23) * w1in2inv;
        const float squareDist2 = (u2 - u1in2) * (u2 - u1in2) + (v2 - v1in2) * (v2 - v1in2);
        const float chiSquare2 = squareDist2 * invSigmaSquare;
​
        // step5. 离群点标记上,非离群点累加计算得分
        if (chiSquare2 > th)
            bIn = false;
        else
            score += th - chiSquare2;
        
        
        if (bIn)
            vbMatchesInliers[i] = true;
        else
            vbMatchesInliers[i] = false;
    }
    return score;
}

6.2.6 归一化: Normalize()

使用均值和一阶中心矩归一化,归一化可以增强计算稳定性.

void Initializer::Normalize(const vector <cv::KeyPoint> &vKeys, vector <cv::Point2f> &vNormalizedPoints, cv::Mat &T) {
    // step1. 计算均值
    float meanX = 0;
    float meanY = 0;
    for (int i = 0; i < N; i++) {
        meanX += vKeys[i].pt.x;
        meanY += vKeys[i].pt.y;
    }
    meanX = meanX / N;
    meanY = meanY / N;
​
    // step2. 计算一阶中心矩
    float meanDevX = 0;
    float meanDevY = 0;
    for (int i = 0; i < N; i++) {
        vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;
        vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;
        meanDevX += fabs(vNormalizedPoints[i].x);
        meanDevY += fabs(vNormalizedPoints[i].y);
    }
    meanDevX = meanDevX / N;
    meanDevY = meanDevY / N;
    float sX = 1.0 / meanDevX;
    float sY = 1.0 / meanDevY;
​
    // step3. 进行归一化
    for (int i = 0; i < N; i++) {
        vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
        vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
    }
​
    // 记录归一化参数,以便后续恢复尺度
    T = cv::Mat::eye(3, 3, CV_32F);
    T.at<float>(0, 0) = sX;
    T.at<float>(1, 1) = sY;
    T.at<float>(0, 2) = -meanX * sX;
    T.at<float>(1, 2) = -meanY * sY;
}

6.3 使用基础矩阵F和单应矩阵H恢复运动

6.3.1 使用基础矩阵F恢复运动: ReconstructF()

使用基础矩阵F分解R、t,数学上会得到四个可能的解,因此分解后调用函数Initializer::CheckRT()检验分解结果,取相机前方成功三角化数目最多的一组解.

bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K, cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated) {
    
    int N = 0;
    for (size_t i = 0, iend = vbMatchesInliers.size(); i < iend; i++)
        if (vbMatchesInliers[i]) N++;
​
    // step1. 根据基础矩阵F推算本质矩阵E
    cv::Mat E21 = K.t() * F21 * K;
​
    // step2. 分解本质矩阵E,得到R,t    
    cv::Mat R1, R2, t;
    DecomposeE(E21, R1, R2, t);
    cv::Mat t1 = t;
    cv::Mat t2 = -t;
    
    // step3. 检验分解出的4对R,t
    vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
    vector<bool> vbTriangulated1, vbTriangulated2, vbTriangulated3, vbTriangulated4;
    float parallax1, parallax2, parallax3, parallax4;
    int nGood1 = CheckRT(R1, t1, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D1, 4.0 * mSigma2, vbTriangulated1, parallax1);
    int nGood2 = CheckRT(R2, t1, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D2, 4.0 * mSigma2, vbTriangulated2, parallax2);
    int nGood3 = CheckRT(R1, t2, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D3, 4.0 * mSigma2, vbTriangulated3, parallax3);
    int nGood4 = CheckRT(R2, t2, mvKeys1, mvKeys2, mvMatches12, vbMatchesInliers, K, vP3D4, 4.0 * mSigma2, vbTriangulated4, parallax4);
    int maxGood = max(nGood1, max(nGood2, max(nGood3, nGood4)));
    R21 = cv::Mat();
    t21 = cv::Mat();
    int nMinGood = max(static_cast<int>(0.9 * N), minTriangulated);
​
    // step4. ratio test,最优分解应有区分度
    int nsimilar = 0;
    if (nGood1 > 0.7 * maxGood)
        nsimilar++;
    if (nGood2 > 0.7 * maxGood)
        nsimilar++;
    if (nGood3 > 0.7 * maxGood)
        nsimilar++;
    if (nGood4 > 0.7 * maxGood)
        nsimilar++;
    if (maxGood < nMinGood || nsimilar > 1) {
        return false;
    }
​
    // step5. 选择记录最佳结果,检验三角化出的特征点数和视差角
    if (maxGood == nGood1) {
        if (parallax1 > minParallax) {
            vP3D = vP3D1;
            vbTriangulated = vbTriangulated1;
            R1.copyTo(R21);
            t1.copyTo(t21);
            return true;
        }
    } else if (maxGood == nGood2) {
        if (parallax2 > minParallax) {
            vP3D = vP3D2;
            vbTriangulated = vbTriangulated2;
​
            R2.copyTo(R21);
            t1.copyTo(t21);
            return true;
        }
    } else if (maxGood == nGood3) {
        if (parallax3 > minParallax) {
            vP3D = vP3D3;
            vbTriangulated = vbTriangulated3;
​
            R1.copyTo(R21);
            t2.copyTo(t21);
            return true;
        }
    } else if (maxGood == nGood4) {
        if (parallax4 > minParallax) {
            vP3D = vP3D4;
            vbTriangulated = vbTriangulated4;
​
            R2.copyTo(R21);
            t2.copyTo(t21);
            return true;
        }
    }
​
    return false;
}

6.3.2 使用单应矩阵H恢复运动: ReconstructH()

6.3.3 检验分解结果R,t

通过成功三角化的特征点个数判断分解结果的好坏: 若某特征点的重投影误差小于4且视差角大于0.36°,则认为该特征点三角化成功

int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2, const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers, const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float &parallax) {
   
    const float fx = K.at<float>(0, 0);
    const float fy = K.at<float>(1, 1);
    const float cx = K.at<float>(0, 2);
    const float cy = K.at<float>(1, 2);
​
    vbGood = vector<bool>(vKeys1.size(), false);
    vP3D.resize(vKeys1.size());
​
    vector<float> vCosParallax;
    vCosParallax.reserve(vKeys1.size());
​
    // step1. 以相机1光心为世界坐标系,计算相机的投影矩阵和光心位置
    cv::Mat P1(3, 4, CV_32F, cv::Scalar(0));        // P1表示相机1投影矩阵, K[I|0]
    K.copyTo(P1.rowRange(0, 3).colRange(0, 3));     
    cv::Mat O1 = cv::Mat::zeros(3, 1, CV_32F);      // O1表示世界坐标下相机1光心位置, O1=0
    cv::Mat P2(3, 4, CV_32F);                       // P2表示相机2投影矩阵, K[R|t]
    R.copyTo(P2.rowRange(0, 3).colRange(0, 3));
    t.copyTo(P2.rowRange(0, 3).col(3));             // O1表示世界坐标下相机2光心位置, O2=-R'*t
    P2 = K * P2;
    cv::Mat O2 = -R.t() * t;
​
    // 遍历所有特征点对
    int nGood = 0;
    for (size_t i = 0, iend = vMatches12.size(); i < iend; i++) {
        // step2. 三角化地图点
        const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];
        const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
        cv::Mat p3dC1;
        Triangulate(kp1, kp2, P1, P2, p3dC1);
​
        // step3. 检查三角化坐标点合法性: 
        // step3.1. 正确三角化的地图点深度值应为正数且视差角足够大
        cv::Mat normal1 = p3dC1 - O1;
        float dist1 = cv::norm(normal1);
        cv::Mat normal2 = p3dC1 - O2;
        float dist2 = cv::norm(normal2);
        float cosParallax = normal1.dot(normal2) / (dist1 * dist2);
        if (p3dC1.at<float>(2) <= 0 && cosParallax < 0.99998)
            continue;
        if (p3dC2.at<float>(2) <= 0 && cosParallax < 0.99998)
            continue;
        
        // step3.2. 正确三角化的地图点重投影误差应足够小
        float im1x, im1y;
        float invZ1 = 1.0 / p3dC1.at<float>(2);
        im1x = fx * p3dC1.at<float>(0) * invZ1 + cx;
        im1y = fy * p3dC1.at<float>(1) * invZ1 + cy;
        float squareError1 = (im1x - kp1.pt.x) * (im1x - kp1.pt.x) + (im1y - kp1.pt.y) * (im1y - kp1.pt.y);
        if (squareError1 > th2)
            continue;
​
        float im2x, im2y;
        float invZ2 = 1.0 / p3dC2.at<float>(2);
        im2x = fx * p3dC2.at<float>(0) * invZ2 + cx;
        im2y = fy * p3dC2.at<float>(1) * invZ2 + cy;
        float squareError2 = (im2x - kp2.pt.x) * (im2x - kp2.pt.x) + (im2y - kp2.pt.y) * (im2y - kp2.pt.y);
        if (squareError2 > th2)
            continue;
​
        // step4. 记录通过检验的地图点
        vCosParallax.push_back(cosParallax);
        vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0), p3dC1.at<float>(1), p3dC1.at<float>(2));
        nGood++;
    }
​
    // step5. 记录三角化过程中的较小(第50个视差角)
    if (nGood > 0) {
        sort(vCosParallax.begin(), vCosParallax.end());
        size_t idx = min(50, int(vCosParallax.size() - 1));
        parallax = acos(vCosParallax[idx]) * 180 / CV_PI;
    } else
        parallax = 0;
​
    return nGood;
}

SVD求解超定方程

void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D) {
    cv::Mat A(4, 4, CV_32F);
    A.row(0) = kp1.pt.x * P1.row(2) - P1.row(0);
    A.row(1) = kp1.pt.y * P1.row(2) - P1.row(1);
    A.row(2) = kp2.pt.x * P2.row(2) - P2.row(0);
    A.row(3) = kp2.pt.y * P2.row(2) - P2.row(1);
    cv::Mat u, w, vt;
    cv::SVD::compute(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
    x3D = vt.row(3).t();
    x3D = x3D.rowRange(0, 3) / x3D.at<float>(3);
}

6.4 对极几何

6.4.1 本质矩阵、基础矩阵和单应矩阵

设点P在相机1、2坐标系下的坐标分别为、,在相机$X_1,X_2$成像平面下的像素坐标分别为$x_1$、$x_2$,有:
$$\begin{gathered} E=t^{\wedge }R \\ F={K_2}^{T}E{K_1}^T \\ {x_2}^{T}F{x}_1={X_2}^{T}F{X}_1=0 \end{gathered}$$
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jNQlOSbv-1651331512009)(…/AppData/Roaming/Typora/typora-user-images/1628643509263.png)]

• 矩阵的自由为:

  矩阵为大小,自由度最大为;考虑到尺度等价性约束,实际自由度为.

• 矩阵自由度为:

  、待定参数各为,和的待定参数各为,共个待定参数.

  但矩阵为大小,自由度最大为;考虑到尺度等价性和行列式两个约束,实际自由度为.

• 矩阵的自由度为:

  和的自由度各为,自由度最大为,考虑到尺度等价性约束,实际自由度为.

• 矩阵的秩为,从两个方面来解释:

  • ,因此
  • 对于某对非坐标、,有成立,说明方程存在非零解,即矩阵不满秩.

6.4.2 极线与极点

---

使用基础矩阵F恢复运动
ReconstructF()使用单应矩阵H恢复运动
ReconstructH()计算基础矩阵F及其卡方检验得分
FindFundamental()计算单应矩阵H及其卡方检验得分
FindHomography()RANSAC循环RANSAC循环循环迭代8个分解结果循环迭代4个分解结果单应矩阵分数占比>0.4单应矩阵分数占比<0.4分解得到R,t通过三角化检验R,t
CheckRT()三角化地图点
Triangulate()分解得到R,t通过三角化检验R,t
CheckRT()三角化地图点
Triangulate()特征点坐标归一化
Normalize()八点法计算基础矩阵F
ComputeF21()卡方检验
CheckFundamental()特征点坐标归一化
Normalize()计算单应矩阵H
ComputeH21()卡方检验
CheckHomography()设置RANSAC用到的点对判断两个矩阵得分之比若三角化得到的点数足够多且视差角足够大,则初始化成功

单应矩阵分数占比>0.4单应矩阵分数占比<0.4设置RANSAC用到的点对计算单应矩阵H及其卡方检验得分
FindHomography()计算基础矩阵F及其卡方检验得分
FindFundamental()判断两个矩阵得分之比使用单应矩阵H恢复运动
ReconstructH()使用基础矩阵F恢复运动
ReconstructF()若三角化得到的点数足够多且视差角足够大,则初始化成功

计算基础矩阵F及其卡方检验得分
FindFundamental()RANSAC循环特征点坐标归一化
Normalize()八点法计算基础矩阵F
ComputeF21()卡方检验
CheckFundamental()

计算单应矩阵H及其卡方检验得分
FindHomography()RANSAC循环特征点坐标归一化
Normalize()计算单应矩阵H
ComputeH21()卡方检验
CheckHomography()

使用基础矩阵F恢复运动
ReconstructF()循环迭代4个分解结果分解得到R,t通过三角化检验R,t
CheckRT()三角化地图点
Triangulate()

使用单应矩阵H恢复运动
ReconstructH()循环迭代8个分解结果分解得到R,t通过三角化检验R,t
CheckRT()三角化地图点
Triangulate()

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

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

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

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

7. ORB-SLAM2代码详解07_跟踪线程Tracking

7.1 各成员函数/变量

7.1.1 跟踪状态

Tracking类中定义枚举类型eTrackingState,用于表示跟踪状态,其可能的取值如下

值	意义
SYSTEM_NOT_READY	系统没有准备好,一般就是在启动后加载配置文件和词典文件时候的状态
NO_IMAGES_YET	还没有接收到输入图像
NOT_INITIALIZED	接收到图像但未初始化成功
OK	跟踪成功
LOST	跟踪失败

---

Tracking类的成员变量mState和mLastProcessedState分别表示当前帧的跟踪状态和上一帧的跟踪状态.

成员变量	访问控制	意义
eTrackingState mState	public	当前帧mCurrentFrame的跟踪状态
eTrackingState mLastProcessedState	public	前一帧mLastFrame的跟踪状态

跟踪状态转移图如下:

7.1.2 初始化

成员函数/变量	访问控制	意义
Frame mCurrentFrame	public	当前帧
KeyFrame* mpReferenceKF	protected	参考关键帧 初始化成功的帧会被设为参考关键帧
std::vector mvpLocalKeyFrames	protected	局部关键帧列表,初始化成功后向其中添加局部关键帧
std::vector mvpLocalMapPoints	protected	局部地图点列表,初始化成功后向其中添加局部地图点

初始化用于SLAM系统刚开始接收到图像的几帧,初始化成功之后就进入正常的跟踪操作.

Tracking类主函数Tracking::Track()检查到当前系统的跟踪状态mState为NOT_INITIALIZED时,就会进行初始化.

void Tracking::Track() {
    
    // ...
    
    unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
​
    // step1. 若还没初始化,则尝试初始化
    if (mState == NOT_INITIALIZED) {
        if (mSensor == System::STEREO || mSensor == System::RGBD)
            StereoInitialization();
        else
            MonocularInitialization();
        if (mState != OK)
            return;
    } 
    
    // ...
}

7.2 单目相机初始化: MonocularInitialization()

成员函数/变量	访问控制	意义
void MonocularInitialization()	protected	单目相机初始化
void CreateInitialMapMonocular()	protected	单目初始化成功后建立初始局部地图
Initializer* mpInitializer	protected	单目初始化器
Frame mInitialFrame	public	单目初始化参考帧(实际上就是前一帧)
std::vector mvIniP3D	public	单目初始化中三角化得到的地图点坐标
std::vector mvbPrevMatched	public	单目初始化参考帧地图点
std::vector mvIniMatches	public	单目初始化中参考帧与当前帧的匹配关系

单目相机初始化条件: 连续两帧间成功三角化超过100个点,则初始化成功.

void Tracking::MonocularInitialization() {
    // step1. 若单目初始化器还没创建,则创建初始化器
    if (!mpInitializer) {
        if (mCurrentFrame.mvKeys.size() > 100) {
            mInitialFrame = Frame(mCurrentFrame);
            mLastFrame = Frame(mCurrentFrame);
            mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
            for (size_t i = 0; i < mCurrentFrame.mvKeysUn.size(); i++)
                mvbPrevMatched[i] = mCurrentFrame.mvKeysUn[i].pt;
            mpInitializer = new Initializer(mCurrentFrame, 1.0, 200);
            fill(mvIniMatches.begin(), mvIniMatches.end(), -1);
            return;
        }
    } else  {
        // step2. 若上一帧特征点足够,但当前帧特征点太少,则匹配失败,删除初始化器
        if ((int) mCurrentFrame.mvKeys.size() <= 100) {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer *>(NULL);
            fill(mvIniMatches.begin(), mvIniMatches.end(), -1);
            return;
        }
​
        // step3. 在mInitialFrame和mLastFrame间进行匹配搜索
        ORBmatcher matcher(0.9, true);
        int nmatches = matcher.SearchForInitialization(mInitialFrame, mCurrentFrame, mvbPrevMatched, mvIniMatches, 100);
​
        // step4. 匹配的特征点数目太少,则匹配失败,删除初始化器
        if (nmatches < 100) {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer *>(NULL);
            return;
        }
​
        // step5. 进行单目初始化
        cv::Mat Rcw; 
        cv::Mat tcw; 
        vector<bool> vbTriangulated;
        if (mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)) {
            // step6. 单目初始化成功后,删除未成功三角化的匹配点对
            for (size_t i = 0, iend = mvIniMatches.size(); i < iend; i++) {
                if (mvIniMatches[i] >= 0 && !vbTriangulated[i]) {
                    mvIniMatches[i] = -1;
                    nmatches--;
                }
            }
            
            // step7. 创建初始化地图,以mInitialFrame为参考系
            cv::Mat Tcw = cv::Mat::eye(4, 4, CV_32F);
            Rcw.copyTo(Tcw.rowRange(0, 3).colRange(0, 3));
            tcw.copyTo(Tcw.rowRange(0, 3).col(3));
            mInitialFrame.SetPose(cv::Mat::eye(4, 4, CV_32F));
            mCurrentFrame.SetPose(Tcw);
            CreateInitialMapMonocular();
        }
    }
}

单目初始化成功后调用函数CreateInitialMapMonocular()创建初始化地图

void Tracking::CreateInitialMapMonocular() {
    // mInitialFrame 和 mCurrentFrame 是最早的两个关键帧
    KeyFrame *pKFini = new KeyFrame(mInitialFrame, mpMap, mpKeyFrameDB);  
    KeyFrame *pKFcur = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);  
​
    // step1. 计算两个关键帧的词袋
    pKFini->ComputeBoW();
    pKFcur->ComputeBoW();
​
    // step2. 将两个关键帧插入地图
    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);
​
    // step3. 处理所有地图点
    for (size_t i = 0; i < mvIniMatches.size(); i++) {
        // 创建并添加地图点
        MapPoint *pMP = new MapPoint(mvIniP3D[i], pKFcur, mpMap);       
        mpMap->AddMapPoint(pMP);
        // 添加关键帧到地图点的观测
        pKFini->AddMapPoint(pMP, i);
        pKFcur->AddMapPoint(pMP, mvIniMatches[i]);
        // 添加地图点到关键帧的观测
        pMP->AddObservation(pKFini, i);
        pMP->AddObservation(pKFcur, mvIniMatches[i]);
        // 计算地图点描述子并更新平均观测数据
        pMP->ComputeDistinctiveDescriptors();
        pMP->UpdateNormalAndDepth();
    }
​
    // 基于观测到的地图点,更新关键帧共视图
    pKFini->UpdateConnections();
    pKFcur->UpdateConnections();
​
    // step4. 全局BA: 优化所有关键帧位姿和地图点
    Optimizer::GlobalBundleAdjustemnt(mpMap, 20);
​
    // step5. 将两帧间的平移尺度归一化(以场景的中值深度为参考)
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f / medianDepth;
    cv::Mat Tc2w = pKFcur->GetPose();
    Tc2w.col(3).rowRange(0, 3) = Tc2w.col(3).rowRange(0, 3) * invMedianDepth;
    pKFcur->SetPose(Tc2w);
​
    // step6. 将坐标点尺度也归一化
    vector<MapPoint *> vpAllMapPoints = pKFini->GetMapPointMatches();
    for (size_t iMP = 0; iMP < vpAllMapPoints.size(); iMP++) {
        if (vpAllMapPoints[iMP]) {
            MapPoint *pMP = vpAllMapPoints[iMP];
            pMP->SetWorldPos(pMP->GetWorldPos() * invMedianDepth);
        }
    }
​
    // step7. 将关键帧插入局部地图,更新归一化后的位姿和地图点坐标  
    mpLocalMapper->InsertKeyFrame(pKFini);
    mpLocalMapper->InsertKeyFrame(pKFcur);
    mCurrentFrame.SetPose(pKFcur->GetPose());
    mnLastKeyFrameId = mCurrentFrame.mnId;
    mpLastKeyFrame = pKFcur;
    mvpLocalKeyFrames.push_back(pKFcur);
    mvpLocalKeyFrames.push_back(pKFini);
    mvpLocalMapPoints = mpMap->GetAllMapPoints();
    mpReferenceKF = pKFcur;
    mCurrentFrame.mpReferenceKF = pKFcur;
    mLastFrame = Frame(mCurrentFrame);
    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
    mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());
    mpMap->mvpKeyFrameOrigins.push_back(pKFini);
    
    // step8. 更新跟踪状态变量mState
    mState = OK;
}

7.3 双目/RGBD相机初始化: StereoInitialization()

成员函数/变量	访问控制	意义
void StereoInitialization()	protected	双目/RGBD相机初始化

双目/RGBD相机的要求就宽松多了,只要左目图像能找到多于500个特征点,就算是初始化成功.

函数StereoInitialization()内部既完成了初始化,又构建了初始化局部地图.

void Tracking::StereoInitialization() {
    if (mCurrentFrame.N > 500) {
        
        // 基于当前帧构造初始关键帧
        mCurrentFrame.SetPose(cv::Mat::eye(4, 4, CV_32F));
        KeyFrame *pKFini = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
        mpMap->AddKeyFrame(pKFini);
        mpLocalMapper->InsertKeyFrame(pKFini);
        // 构造地图点
        for (int i = 0; i < mCurrentFrame.N; i++) {
            float z = mCurrentFrame.mvDepth[i];
            if (z > 0) {
                cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
                MapPoint *pNewMP = new MapPoint(x3D, pKFini, mpMap);
                pNewMP->AddObservation(pKFini, i);
                pNewMP->ComputeDistinctiveDescriptors();
                pNewMP->UpdateNormalAndDepth();
                mpMap->AddMapPoint(pNewMP);
                pKFini->AddMapPoint(pNewMP, i);
                mCurrentFrame.mvpMapPoints[i] = pNewMP;
            }
        }
​
        // 构造局部地图
        mvpLocalKeyFrames.push_back(pKFini);
        mvpLocalMapPoints = mpMap->GetAllMapPoints();
        mpReferenceKF = pKFini;
        mCurrentFrame.mpReferenceKF = pKFini;
        
        // 更新跟踪状态变量mState
        mState = OK;
    }
}

7.4 初始位姿估计

当Tracking线程接收到一帧图像后,会先估计其初始位姿,再根据估计出的初始位姿跟踪局部地图并进一步优化位姿.

初始位姿估计有三种手段:

• 根据恒速运动模型估计位姿TrackWithMotionModel()
• 根据参考帧估计位姿TrackReferenceKeyFrame()
• 通过重定位估计位姿Relocalization()

void Tracking::Track() {
    
    // ...
    
    unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
​
    // step1. 若还没初始化,则尝试初始化
    if (mState == NOT_INITIALIZED) {
        // 初始化
    } else {
        // step2. 若系统已初始化,就进行跟踪(或重定位)
        bool bOK;
​
        // step2.1. 符合条件时,优先根据运动模型跟踪,如运动模型跟踪失败,就根据参考帧进行跟踪
        if (mState == OK) {
            if (mVelocity.empty() || mCurrentFrame.mnId < mnLastRelocFrameId + 2) {     // 判断当前关键帧是否具有较稳定的速度
                bOK = TrackReferenceKeyFrame();
            } else {
                bOK = TrackWithMotionModel();
                if (!bOK)
                    bOK = TrackReferenceKeyFrame();
            }
        } else {
            // step2.2. 若上一帧没跟踪丢失,则这一帧重定位
            bOK = Relocalization();
        }
        
        // ...
        if (bOK)
            mState = OK;
        else
            mState = LOST;
    }
    
    // ...
}

7.5 根据恒速运动模型估计初始位姿: TrackWithMotionModel()

恒速运动模型假定连续几帧间的运动速度是恒定的;基于此假设,根据运动速度mVelocity和上一帧的位姿mLastFrame.mTcw计算出本帧位姿的估计值,再进行位姿优化.

成员变量mVelocity保存前一帧的速度,主函数Tracking::Track()中调用完函数Tracking::TrackLocalMap()更新局部地图和当前帧位姿后,就计算速度并赋值给mVelocity.

成员函数/变量	访问控制	意义
TrackWithMotionModel()	protected	根据恒速运动模型估计初始位姿
Frame mLastFrame	protected	前一帧,TrackWithMotionModel()与该帧匹配搜索关键点
cv::Mat mVelocity	protected	相机前一帧运动速度,跟踪完局部地图后更新该成员变量
list mlpTemporalPoints	protected	双目/RGBD相机输入时,为前一帧生成的临时地图点 跟踪成功后该容器会被清空,其中的地图点会被删除

bool Tracking::TrackWithMotionModel() {
    ORBmatcher matcher(0.9, true);
​
    // step1. 更新上一帧的位姿,对于双目/RGBD相机,还会生成临时地图点
    UpdateLastFrame();
    // step2. 根据运动模型设置初始位姿估计值
    mCurrentFrame.SetPose(mVelocity * mLastFrame.mTcw);
    fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast<MapPoint *>(NULL));
​
    // step3. 根据初始位姿估计值进行投影匹配
    int th;
    if (mSensor != System::STEREO)
        th = 15;//单目
    else
        th = 7;//双目
    // step3.1. 寻找匹配特征点
    int nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, th, mSensor == System::MONOCULAR);
    // step3.2. 匹配特征点数目太少就放宽条件重新搜索匹配
    if (nmatches < 20) {    
        fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(), static_cast<MapPoint *>(NULL));
        nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, 2 * th, mSensor == System::MONOCULAR);
    }
    // step3.3. 实在找不到足够的匹配特征点,运动模型跟踪失败
    if (nmatches < 20)
        return false;
​
    // step4. 位姿BA: 只优化当前帧位姿
    Optimizer::PoseOptimization(&mCurrentFrame);
​
    // step5. 剔除外点
    int nmatchesMap = 0;
    for (int i = 0; i < mCurrentFrame.N; i++) {
        if (mCurrentFrame.mvpMapPoints[i]) {
            if (mCurrentFrame.mvbOutlier[i]) {
                MapPoint *pMP = mCurrentFrame.mvpMapPoints[i];
                mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);
                mCurrentFrame.mvbOutlier[i] = false;
            } else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0)
                nmatchesMap++;
        }
    }
​
    // step6. 匹配的地图点数超过10个就认为是跟踪成功
    return nmatchesMap >= 10;
}

为保证位姿估计的准确性,对于双目/RGBD相机,为前一帧生成临时地图点.

void Tracking::UpdateLastFrame() {
    
    // step1. 根据参考关键帧确定当前帧的位姿,使用关键帧是因为参考关键帧位姿更准确
    KeyFrame *pRef = mLastFrame.mpReferenceKF;
    cv::Mat Tlr = mlRelativeFramePoses.back();
    mLastFrame.SetPose(Tlr * pRef->GetPose());
​
    
    // step2. 对于双目/RGBD相机,生成新的临时地图点
    if (mnLastKeyFrameId == mLastFrame.mnId || mSensor == System::MONOCULAR)
        return;
    // step2.1. 将上一帧种存在深度的特征点按深度从小到大排序
    vector<pair<float, int> > vDepthIdx;
    vDepthIdx.reserve(mLastFrame.N);
    for (int i = 0; i < mLastFrame.N; i++) {
        float z = mLastFrame.mvDepth[i];
        if (z > 0) {
            vDepthIdx.push_back(make_pair(z, i));
        }
    }
    sort(vDepthIdx.begin(), vDepthIdx.end());
    // step2.2. 将上一帧中没三角化的特征点三角化出来,作为临时地图点
    int nPoints = 0;        // 统计处理了多少特征点
    for (size_t j = 0; j < vDepthIdx.size(); j++) {
        int i = vDepthIdx[j].second;
        bool bCreateNew = false;
        MapPoint *pMP = mLastFrame.mvpMapPoints[i];
        if (!pMP || pMP->Observations() < 1) {
            bCreateNew = true;
        }
        // step2.3. 这些临时地图点在CreateNewKeyFrame之前会被删除,因此不用添加其他复杂属性
        if (bCreateNew) {
            cv::Mat x3D = mLastFrame.UnprojectStereo(i);
            MapPoint *pNewMP = new MapPoint(x3D, mpMap, &mLastFrame, i);
            mLastFrame.mvpMapPoints[i] = pNewMP;
            mlpTemporalPoints.push_back(pNewMP);
        }
        // step2.4. 临时地图点数目足够(多于100个)或者深度太大(深度越大位置越不准确),就停止生成临时地图点
        if (vDepthIdx[j].first > mThDepth && nPoints > 100)
            break;
        nPoints++;
    }
}

7.6 根据参考帧估计位姿: TrackReferenceKeyFrame()

成员变量mpReferenceKF保存Tracking线程当前的参考关键帧,参考关键帧有两个来源:

• 每当Tracking线程创建一个新的参考关键帧时,就将其设为参考关键帧.
• 跟踪局部地图的函数Tracking::TrackLocalMap()内部会将与当前帧共视点最多的局部关键帧设为参考关键帧.

成员函数/变量	访问控制	意义
TrackReferenceKeyFrame()	protected	根据参考帧估计位姿
KeyFrame* mpReferenceKF	protected	参考关键帧,TrackReferenceKeyFrame()与该关键帧匹配搜索关键点

bool Tracking::TrackReferenceKeyFrame() {
    
    // step1. 根据当前帧描述子计算词袋
    mCurrentFrame.ComputeBoW();
    
    // step2. 根据词袋寻找匹配
    ORBmatcher matcher(0.7, true);
    vector<MapPoint *> vpMapPointMatches;
    int nmatches = matcher.SearchByBoW(mpReferenceKF, mCurrentFrame, vpMapPointMatches);
    if (nmatches < 15)  // 词袋匹配过少则跟踪失败
        return false;
​
    // step3. 将上一帧位姿设为初始位姿估计值
    mCurrentFrame.SetPose(mLastFrame.mTcw); 
    mCurrentFrame.mvpMapPoints = vpMapPointMatches;
    
    // step4. 位姿BA: 只优化当前帧位姿
    Optimizer::PoseOptimization(&mCurrentFrame);
​
    // step5. 剔除外点
    int nmatchesMap = 0;
    for (int i = 0; i < mCurrentFrame.N; i++) {
        if (mCurrentFrame.mvpMapPoints[i]) {
            if (mCurrentFrame.mvbOutlier[i]) {
                MapPoint *pMP = mCurrentFrame.mvpMapPoints[i];
                mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);
                mCurrentFrame.mvbOutlier[i] = false;
            } else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0)
                nmatchesMap++;
        }
    }
    
    // step6. 匹配的地图点数超过10个就认为是跟踪成功
    return nmatchesMap >= 10;
}

思考: 为什么函数Tracking::TrackReferenceKeyFrame()没有检查当前参考帧mpReferenceKF是否被LocalMapping线程删除了?

回答: 因为LocalMapping线程剔除冗余关键帧函数LocalMapping::KeyFrameCulling()不会删除最新的参考帧,有可能被删除的都是之前的参考帧.

7.7 通过重定位估计位姿: Relocalization()

当TrackWithMotionModel()和TrackReferenceKeyFrame()都失败后,就会调用函数Relocalization()通过重定位估计位姿.

bool Tracking::Relocalization() {
    // step1. 根据当前帧描述子计算词袋
    mCurrentFrame.ComputeBoW();
​
    // step2. 根据词袋找到当前帧的候选参考关键帧
    vector<KeyFrame *> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);    
    const int nKFs = vpCandidateKFs.size();
    vector<PnPsolver *> vpPnPsolvers;
    vpPnPsolvers.resize(nKFs);
​
    // step3. 遍历所有的候选参考关键帧,通过BOW进行快速匹配,用匹配结果初始化PnPsolver
    vector<vector<MapPoint *> > vvpMapPointMatches;
    vector<bool> vbDiscarded;
    ORBmatcher matcher(0.75, true);
    int nCandidates = 0;
    for (int i = 0; i < nKFs; i++) {
        int nmatches = matcher.SearchByBoW(pKF, mCurrentFrame, vvpMapPointMatches[i]);
        PnPsolver *pSolver = new PnPsolver(mCurrentFrame, vvpMapPointMatches[i]);
        pSolver->SetRansacParameters(0.99, 10, 300, 4, 0.5, 5.991);
        vpPnPsolvers[i] = pSolver;
        nCandidates++;
    }
​
    // step4. 使用PnPsolver估计初始位姿,并根据初始位姿获取足够的匹配特征点
    bool bMatch = false;
    ORBmatcher matcher2(0.9, true);
​
    while (nCandidates > 0 && !bMatch) {
        for (int i = 0; i < nKFs; i++) {
            vector<bool> vbInliers;
            int nInliers;
            bool bNoMore;
            // step4.1. 通过PnPsolver估计初始位姿
            PnPsolver *pSolver = vpPnPsolvers[i];
            cv::Mat Tcw = pSolver->iterate(5, bNoMore, vbInliers, nInliers);
            if (bNoMore) {
                vbDiscarded[i] = true;
                nCandidates--;
            }
            // step4.2. 位姿BA: 只优化当前帧位姿
            Tcw.copyTo(mCurrentFrame.mTcw);
            set<MapPoint *> sFound;
            const int np = vbInliers.size();
            for (int j = 0; j < np; j++) {
                if (vbInliers[j]) {
                    mCurrentFrame.mvpMapPoints[j] = vvpMapPointMatches[i][j];
                    sFound.insert(vvpMapPointMatches[i][j]);
                } else
                    mCurrentFrame.mvpMapPoints[j] = NULL;
            }
            int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
​
            // step4.3. 剔除外点
            for (int io = 0; io < mCurrentFrame.N; io++)
                if (mCurrentFrame.mvbOutlier[io])
                    mCurrentFrame.mvpMapPoints[io] = static_cast<MapPoint *>(NULL);
            
            // step4.4. 若匹配特征点数目太少,则尝试第2次进行特征匹配+位姿优化
            if (nGood < 50) {
                int nadditional = matcher2.SearchByProjection(mCurrentFrame, vpCandidateKFs[i], sFound, 10, 100);
                if (nadditional + nGood >= 50) {
                    nGood = Optimizer::PoseOptimization(&mCurrentFrame);
                    // step4.5. 若匹配特征点数目太少,则尝试第3次进行特征匹配+位姿优化
                    if (nGood > 30 && nGood < 50) {
                        sFound.clear();
                        for (int ip = 0; ip < mCurrentFrame.N; ip++)
                            if (mCurrentFrame.mvpMapPoints[ip])
                                sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
                        nadditional = matcher2.SearchByProjection(mCurrentFrame, vpCandidateKFs[i], sFound, 3, 64);
                        if (nGood + nadditional >= 50) {
                            nGood = Optimizer::PoseOptimization(&mCurrentFrame);
                            for (int io = 0; io < mCurrentFrame.N; io++)
                                if (mCurrentFrame.mvbOutlier[io])
                                    mCurrentFrame.mvpMapPoints[io] = NULL;
                        }
                    }
                }
            }
            // step4.6. 若最后匹配数目终于足够了,则跟踪成功
            if (nGood >= 50) {
                bMatch = true;
                break;
            }
        }
    }
    
    // step5. 返回是否跟踪成功
    if (!bMatch) {
        return false;
    } else {
        mnLastRelocFrameId = mCurrentFrame.mnId;
        return true;
    }
}

7.8 跟踪局部地图: TrackLocalMap()

成员函数/变量	访问控制	意义
bool TrackLocalMap()	protected	更新局部地图并优化当前帧位姿
void UpdateLocalMap()	protected	更新局部地图
std::vector mvpLocalKeyFrames	protected	局部关键帧列表
std::vector mvpLocalMapPoints	protected	局部地图点列表
void SearchLocalPoints()	protected	将局部地图点投影到当前帧特征点上

成功估计当前帧的初始位姿后,基于当前位姿更新局部地图并优化当前帧位姿,主要流程:

1. 更新局部地图,包括局部关键帧列表mvpLocalKeyFrames和局部地图点列表mvpLocalMapPoints.

2. 将局部地图点投影到当前帧特征点上.

3. 进行位姿BA,优化当前帧位姿.

4. 更新地图点观测数值,统计内点个数.

   这里的地图点观测数值会被用作LocalMapping线程中LocalMapping::MapPointCulling()函数剔除坏点的标准之一.

5. 根据内点数判断是否跟踪成功.

跟踪局部地图,优化当前帧位姿
TrackLocalMap()更新局部地图
UpdateLocalMap()将局部地图点投影到当前帧特征点上
SearchLocalPoints()对当前帧位姿进行BA优化更新地图点观测根据内点数判断是否跟踪成功更新局部关键帧
UpdateLocalKeyFrames()更新局部地图点
UpdateLocalPoints()

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

bool Tracking::TrackLocalMap() {
    
    // step1. 更新局部地图,包括局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints
    UpdateLocalMap();
​
    // step2. 将局部地图点投影到当前帧特征点上
    SearchLocalPoints();
​
    // step3. 位姿BA: 只优化当前帧位姿
    Optimizer::PoseOptimization(&mCurrentFrame);
​
    // step4. 更新地图点观测,统计内点个数
    // 这里的地图点观测数值会被用作LocalMapping线程中MapPointCulling()函数剔除坏点的标准之一
    mnMatchesInliers = 0;
    for (int i = 0; i < mCurrentFrame.N; i++) {
        if (mCurrentFrame.mvpMapPoints[i]) {
            if (!mCurrentFrame.mvbOutlier[i]) {
                mCurrentFrame.mvpMapPoints[i]->IncreaseFound(); // 位姿估计用到该地图点
                if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0)
                    mnMatchesInliers++;                         
            }
        }
    }
​
    // step5. 判断是否跟踪成功: 若刚发生过重定位,则标准严苛一点,否则标准宽松一点.(防止误闭环)
    if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && mnMatchesInliers < 50)
        return false;
    if (mnMatchesInliers < 30)
        return false;
    else
        return true;
}

函数Tracking::UpdateLocalMap()依次调用函数Tracking::UpdateLocalKeyFrames()更新局部关键帧列表mvpLocalKeyFrames和函数Tracking::UpdateLocalPoints()更新局部地图点列表mvpLocalMapPoints.

void Tracking::UpdateLocalMap() {
    UpdateLocalKeyFrames();         // 更新局部关键帧列表mvpLocalKeyFrames
    UpdateLocalPoints();            // 更新局部地图点列表mvpLocalMapPoints
}

• 函数Tracking::UpdateLocalKeyFrames()内,局部关键帧列表mvpLocalKeyFrames会被清空并重新赋值,包括以下3部分:

  1. 当前地图点的所有共视关键帧.
  2. 1中所有关键帧的父子关键帧.
  3. 1中所有关键帧共视关系前10大的共视关键帧.

  更新完局部关键帧列表mvpLocalKeyFrames后,还将与当前帧共视关系最强的关键帧设为参考关键帧mpReferenceKF.

• 函数Tracking::UpdateLocalPoints()内,局部地图点列表mvpLocalMapPoints会被清空并赋值为局部关键帧列表mvpLocalKeyFrames的所有地图点.

---

函数Tracking::SearchLocalPoints()将局部地图点投影到当前帧特征点上

void Tracking::SearchLocalPoints() {
    // step1. 当前帧地图点已经匹配了特征点
    for (MapPoint *pMP: mCurrentFrame.mvpMapPoints) {
        if (pMP) {
            if (pMP->isBad()) {
                *vit = static_cast<MapPoint *>(NULL);
            } else {
                pMP->IncreaseVisible();
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                pMP->mbTrackInView = false;
            }
        }
    }
    
    // step2. 统计视野内地图点数目 
    int nToMatch = 0;
    for (MapPoint *pMP: mvpLocalMapPoints) {
        if (pMP->mnLastFrameSeen == mCurrentFrame.mnId)
            continue;
        if (pMP->isBad())
            continue;
​
        if (mCurrentFrame.isInFrustum(pMP, 0.5)) {
            pMP->IncreaseVisible();
            nToMatch++;
        }
    }
​
    // Step 3：如果需要进行投影匹配的点的数目大于0，就进行投影匹配
    if (nToMatch > 0) {
        ORBmatcher matcher(0.8);
        int th = 1;
        if (mSensor == System::RGBD) 
            th = 3;
        if (mCurrentFrame.mnId < mnLastRelocFrameId + 2)
            th = 5;
        matcher.SearchByProjection(mCurrentFrame, mvpLocalMapPoints, th);
    }
}

7.9 关键帧的创建

7.9.1 判断是否需要创建新关键帧: NeedNewKeyFrame()

是否生成关键帧,需要考虑以下几个方面:

1. 最近是否进行过重定位,重定位后位姿不会太准,不适合做参考帧.
2. 当前系统的工作状态: 如果LocalMapping线程还有很多KeyFrame没处理的话,不适合再给它增加负担了.
3. 距离上次创建关键帧经过的时间: 如果很长时间没创建关键帧了的话,就要抓紧创建关键帧了.
4. 当前帧的质量: 当前帧观测到的地图点要足够多,同时与参考关键帧的重合程度不能太大.

具体的代码比较乱;不看了.

总体而言,ORB-SLAM2插入关键帧的策略还是比较宽松的,因为后面LocalMapping线程的函数LocalMapping::KeyFrameCulling()会剔除冗余关键帧,因此在系统处理得过来的情况下,要尽量多创建关键帧.

7.9.2 创建新关键帧: CreateNewKeyFrame()

创建新关键帧时,对于双目/RGBD相机输入情况下也创建新地图点.

void Tracking::CreateNewKeyFrame() {
    // step1. 构造关键帧
    KeyFrame *pKF = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
​
    // step2. 将创建出的关键帧设为参考关键帧
    mpReferenceKF = pKF;
    mCurrentFrame.mpReferenceKF = pKF;
​
    // step3. 对于双目/RGBD相机生成新的地图点
    if (mSensor != System::MONOCULAR) {
        mCurrentFrame.UpdatePoseMatrices();
​
        // step3.1. 按深度从小到大排序关键点
        vector<pair<float, int> > vDepthIdx;
        vDepthIdx.reserve(mCurrentFrame.N);
        for (int i = 0; i < mCurrentFrame.N; i++) {
            float z = mCurrentFrame.mvDepth[i];
            if (z > 0) {
                vDepthIdx.push_back(make_pair(z, i));
            }
        }
        
        if (!vDepthIdx.empty()) {
            sort(vDepthIdx.begin(), vDepthIdx.end());
            // step3.2. 找出没对应地图点的特征点,并创建新地图点
            int nPoints = 0;
            for (size_t j = 0; j < vDepthIdx.size(); j++) {
                int i = vDepthIdx[j].second;
                bool bCreateNew = false;
                MapPoint *pMP = mCurrentFrame.mvpMapPoints[i];
                if (!pMP)
                    bCreateNew = true;
                else if (pMP->Observations() < 1) {
                    bCreateNew = true;
                    mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);
                }
                if (bCreateNew) {
                    cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
                    MapPoint *pNewMP = new MapPoint(x3D, pKF, mpMap);
                    pNewMP->AddObservation(pKF, i);
                    pKF->AddMapPoint(pNewMP, i);
                    pNewMP->ComputeDistinctiveDescriptors();
                    pNewMP->UpdateNormalAndDepth();
                    mpMap->AddMapPoint(pNewMP);
                    mCurrentFrame.mvpMapPoints[i] = pNewMP;
                }
                nPoints++;    
                // step3.3. 地图点过多(多于100个)或深度太深(误差太大),则停止生成地图点
                if (vDepthIdx[j].first > mThDepth && nPoints > 100)
                    break;
            }
        }
    }
​
    // step4. 插入关键帧
    mpLocalMapper->InsertKeyFrame(pKF);
    mpLocalMapper->SetNotStop(false);
    mnLastKeyFrameId = mCurrentFrame.mnId;
    mpLastKeyFrame = pKF;
}

7.10 跟踪函数: Track()

主要关注成员变量mState的变化:

值	意义
SYSTEM_NOT_READY	系统没有准备好,一般就是在启动后加载配置文件和词典文件时候的状态
NO_IMAGES_YET	还没有接收到输入图像
NOT_INITIALIZED	接收到图像但未初始化成功
OK	跟踪成功
LOST	跟踪失败

void Tracking::Track() {
    
    // 维护状态
    if (mState == NO_IMAGES_YET) {
        mState = NOT_INITIALIZED;
    }
​
    unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
​
    // step1. 若还没初始化,则尝试初始化
    if (mState == NOT_INITIALIZED) {
        if (mSensor == System::STEREO || mSensor == System::RGBD)
            StereoInitialization();
        else
            MonocularInitialization();
        if (mState != OK)
            return;
    } else {
        // step2. 若系统已初始化,就进行跟踪(或重定位)
        bool bOK;
​
        // step2.1. 符合条件时,优先根据运动模型跟踪,如运动模型跟踪失败,就根据参考帧进行跟踪
        if (mState == OK) {
            CheckReplacedInLastFrame();
            if (mVelocity.empty() || mCurrentFrame.mnId < mnLastRelocFrameId + 2) {
                bOK = TrackReferenceKeyFrame();
            } else {
                bOK = TrackWithMotionModel();
                if (!bOK)
                    bOK = TrackReferenceKeyFrame();
            }
        } else {
            // step2.2. 若上一帧没跟踪丢失,则这一帧重定位
            bOK = Relocalization();
        }
        // step2.3. 设置当前帧的参考关键帧  
        mCurrentFrame.mpReferenceKF = mpReferenceKF;
​
        // step3. 跟踪局部地图,进一步优化当前帧位姿
        //  之前的跟踪过程都是仅根据前面某一帧进行的位姿优化,TrackLocalMap()使用局部地图进行位姿优化
        if (bOK)
            bOK = TrackLocalMap();
​
        // step4. 根据跟踪结果判断跟踪状态
        if (bOK)
            mState = OK;
        else
            mState = LOST;
​
        // step5. 跟踪成功之后的后处理
        if (bOK) {
            // step5.1. 更新恒速运动模型
            if (!mLastFrame.mTcw.empty()) {
                cv::Mat LastTwc = cv::Mat::eye(4, 4, CV_32F);
                mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0, 3).colRange(0, 3));
                mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0, 3).col(3));
                mVelocity = mCurrentFrame.mTcw * LastTwc;   // mVelocity = Tcl = Tcw * Twl
            } else
                mVelocity = cv::Mat();
​
            // step5.2. 剔除失效地图点
            for (int i = 0; i < mCurrentFrame.N; i++) {
                MapPoint *pMP = mCurrentFrame.mvpMapPoints[i];
                if (pMP)
                    if (pMP->Observations() < 1) {
                        mCurrentFrame.mvbOutlier[i] = false;
                        mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);
                    }
            }
​
            // step5.3. 清除恒速模型跟踪中UpdateLastFrame创建的当前帧临时地图点(跟踪失败的话就不清空临时地图点么?)
            for (list<MapPoint *>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end();
                 lit != lend; lit++) {
                MapPoint *pMP = *lit;
                delete pMP;
            }
            mlpTemporalPoints.clear();
​
            // step5.4. 检测并插入关键帧,双目/RGBD相机会创建新地图点
            if (NeedNewKeyFrame())
                CreateNewKeyFrame();
​
            // step5.5. 删除外点
            for (int i = 0; i < mCurrentFrame.N; i++) {
                if (mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
                    mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint *>(NULL);
            }
        }
​
        // step6. 若系统刚启动没多久就跟踪失败的话,就直接重启
        if (mState == LOST) {
            if (mpMap->KeyFramesInMap() <= 5) {
                cout << "Track lost soon after initialisation, reseting..." << endl;
                mpSystem->Reset();
                return;
            }
        }
        
        // step7. 更新上一帧数据
        mLastFrame = Frame(mCurrentFrame);
    }
​
    // step8. 记录位姿信息
    if (!mCurrentFrame.mTcw.empty()) {
        cv::Mat Tcr = mCurrentFrame.mTcw * mCurrentFrame.mpReferenceKF->GetPoseInverse();   // 相对于参考帧Tcr = Tcw * Twr
        mlRelativeFramePoses.push_back(Tcr);
        mlpReferences.push_back(mpReferenceKF);
        mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
        mlbLost.push_back(mState == LOST);
    } else {
        // 跟踪失败就使用上一帧数据作为当前帧记录
        mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
        mlpReferences.push_back(mlpReferences.back());
        mlFrameTimes.push_back(mlFrameTimes.back());
        mlbLost.push_back(mState == LOST);
    }
}

7.11 Tracking流程中的关键问题(暗线)

7.11.1 地图点的创建与删除

1. Tracking线程中初始化过程(Tracking::MonocularInitialization()和Tracking::StereoInitialization())会创建新的地图点.
2. Tracking线程中创建新的关键帧(Tracking::CreateNewKeyFrame())会创建新的地图点.
3. Tracking线程中根据恒速运动模型估计初始位姿(Tracking::TrackWithMotionModel())也会产生临时地图点,但这些临时地图点在跟踪成功后会被马上删除.

所有的非临时地图点都是由关键帧建立的,Tracking::TrackWithMotionModel()中由非关键帧建立的关键点被设为临时关键点,很快会被删掉,仅作增强帧间匹配用,不会对建图产生任何影响.这也不违反只有关键帧才能参与LocalMapping和LoppClosing线程的原则.

思考: 为什么跟踪失败的话不删除这些局部地图点

跟踪失败的话不会产生关键帧,这些地图点也不会被注册进地图,不会对之后的建图产生影响.

思考: 那会不会发生内存泄漏呢?

不会的,因为最后总会有一帧跟踪上,这些临时地图点都被保存在了成员变量mlpTemporalPoints中,跟踪成功后会删除所有之前的临时地图点.

7.11.2 关键帧与地图点间发生关系的时机

• 新创建出来的非临时地图点都会与创建它的关键帧建立双向连接.
• 通过ORBmatcher::SearchByXXX()函数匹配得到的帧点关系只建立单向连接:
  • 只在关键帧中添加了对地图点的观测(将地图点加入到关键帧对象的成员变量mvpMapPoints中了).
  • 没有在地图点中添加对关键帧的观测(地图点的成员变量mObservations中没有该关键帧).

这为后文中LocalMapping线程中函数LocalMapping::ProcessNewKeyFrame()对关键帧中地图点的处理埋下了伏笔.该函数通过检查地图点中是否有对关键点的观测来判断该地图点是否是新生成的.

void LocalMapping::ProcessNewKeyFrame() {
​
    // 遍历关键帧中的地图点
    const vector<MapPoint *> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for (MapPoint *pMP : vpMapPointMatches) {
        if (!pMP->IsInKeyFrame(mpCurrentKeyFrame)) {
            // step3.1. 该地图点是跟踪本关键帧时匹配得到的,在地图点中加入对当前关键帧的观测
            pMP->AddObservation(mpCurrentKeyFrame, i);
            pMP->UpdateNormalAndDepth();
            pMP->ComputeDistinctiveDescriptors();
        } else 
        {
            // step3.2. 该地图点是跟踪本关键帧时新生成的,将其加入容器mlpRecentAddedMapPoints待筛选
            mlpRecentAddedMapPoints.push_back(pMP);
        }
    }
​
    // ...
}

7.12 参考关键帧: mpReferenceKF

• 参考关键帧的用途:
  1. Tracking线程中函数Tracking::TrackReferenceKeyFrame()根据参考关键帧估计初始位姿.
  2. 用于初始化新创建的MapPoint的参考帧mpRefKF,函数MapPoint::UpdateNormalAndDepth()中根据参考关键帧mpRefKF更新地图点的平均观测距离.
• 参考关键帧的指定:
  1. Traking线程中函数Tracking::CreateNewKeyFrame()创建完新关键帧后,会将新创建的关键帧设为参考关键帧.
  2. Tracking线程中函数Tracking::TrackLocalMap()跟踪局部地图过程中调用函数Tracking::UpdateLocalMap(),其中调用函数Tracking::UpdateLocalKeyFrames(),将与当前帧共视程度最高的关键帧设为参考关键帧.

---

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

加载词典和配置文件接受新图像初始化
MonocularInitialization或StereoInitialization()``Track()跟踪成功Track()跟踪失败Relocalization()重定位成功Relocalization()重定位失败SYSTEM_NOT_READY``NO_IMAGES_YET``NOT_INITIALIZED``OK``LOST

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

mState初始化成功初始化失败NOT_INITIALIZEDOK

输入状态Y跟踪失败N跟踪失败跟踪失败跟踪成功跟踪成功跟踪成功OK``LOST

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

8. ORB-SLAM2代码详解08_局部建图线程LocalMapping

8.1 各成员函数/变量

成员函数/变量	访问控制	意义
std::list mlNewKeyFrames	protected	Tracking线程向LocalMapping线程插入关键帧的缓冲队列
void InsertKeyFrame(KeyFrame* pKF)	public	向缓冲队列mlNewKeyFrames内插入关键帧
bool CheckNewKeyFrames()	protected	查看缓冲队列mlNewKeyFrames内是否有待处理的新关键帧
int KeyframesInQueue()	public	查询缓冲队列mlNewKeyFrames内关键帧个数
bool mbAcceptKeyFrames	protected	LocalMapping线程是否愿意接收Tracking线程传来的新关键帧
bool AcceptKeyFrames()	public	mbAcceptKeyFrames的get方法
void SetAcceptKeyFrames(bool flag)	public	mbAcceptKeyFrames的set方法

Tracking线程创建的所有关键帧都被插入到LocalMapping线程的缓冲队列mlNewKeyFrames中.

成员函数mbAcceptKeyFrames表示当前LocalMapping线程是否愿意接收关键帧,这会被Tracking线程函数Tracking::NeedNewKeyFrame()用作是否生产关键帧的参考因素之一;但即使mbAcceptKeyFrames为false,在系统很需要关键帧的情况下Tracking线程函数Tracking::NeedNewKeyFrame()也会决定生成关键帧.

8.2 局部建图主函数: Run()

NYNYN是否请求停止建图设置暂停接收关键帧
SetAcceptKeyFrames(false)是否存在未处理的关键帧
CheckNewKeyFrames处理队列中的关键帧
ProcessNewKeyFrame剔除冗余地图点
MapPointCulling创建新地图点
CreateNewMapPoints是否处理完所有关键帧
CheckNewKeyFrames融合当前关键帧和其共视帧的地图点
SearchInNeighbors局部BA优化
Optimizer::LocalBundleAdjustment剔除冗余关键帧
KeyFrameCulling设置继续接收关键帧
SetAcceptKeyFrames(true)当前线程暂停3秒
std::this_thread::sleep_for(std::chrono::milliseconds(3))

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

函数LocalMapping::Run()是LocalMapping线程的主函数,该函数内部是一个死循环,每3毫秒查询一次当前线程缓冲队列mlNewKeyFrames.若查询到了待处理的新关键帧,就进行查询

void LocalMapping::Run() {
​
    while (1) {
        SetAcceptKeyFrames(false);      // 设置当前LocalMapping线程处于建图状态,不愿意接受Tracking线程传来的关键帧
​
        // step1. 检查缓冲队列内的关键帧 
        if (CheckNewKeyFrames()) {
            // step2. 处理缓冲队列中第一个关键帧
            ProcessNewKeyFrame();
            
            // step3. 剔除劣质地图点
            MapPointCulling();
​
            // step4. 创建新地图点
            CreateNewMapPoints();
​
            if (!CheckNewKeyFrames()) {
                // step5. 将当前关键帧与其共视关键帧地图点融合
                SearchInNeighbors();
                    
                // step6. 局部BA优化: 优化局部地图
                mbAbortBA = false;  
                Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame, &mbAbortBA, mpMap);
​
                // step7. 剔除冗余关键帧
                KeyFrameCulling();
            }
​
            // step8. 将当前关键帧加入闭环检测中
            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
        } 
        
        SetAcceptKeyFrames(true);       // 设置当前LocalMapping线程处于空闲状态,愿意接受Tracking线程传来的关键帧
​
        // 线程暂停3毫秒再开启下一轮查询
        std::this_thread::sleep_for(std::chrono::milliseconds(3));
    }
}

8.3 处理队列中第一个关键帧: ProcessNewKeyFrame()

在第3步中处理当前关键点时比较有意思,通过判断该地图点是否观测到当前关键帧(pMP->IsInKeyFrame(mpCurrentKeyFrame))来判断该地图点是否是当前关键帧中新生成的.

• 若地图点是本关键帧跟踪过程中匹配得到的(Tracking::TrackWithMotionModel()、Tracking::TrackReferenceKeyFrame()、Tracking::Relocalization()和Tracking::SearchLocalPoints()中调用了ORBmatcher::SearchByProjection()和ORBmatcher::SearchByBoW()方法),则是之前关键帧中创建的地图点,只需添加其对当前帧的观测即可.
• 若地图点是本关键帧跟踪过程中新生成的(包括:1.单目或双目初始化Tracking::MonocularInitialization()、Tracking::StereoInitialization();2.创建新关键帧Tracking::CreateNewKeyFrame()),则该地图点中有对当前关键帧的观测,是新生成的地图点,放入容器mlNewKeyFrames中供LocalMapping::MapPointCulling()函数筛选.

void LocalMapping::ProcessNewKeyFrame() {
​
    // step1. 取出队列头的关键帧 
    {
        unique_lock<mutex> lock(mMutexNewKFs);
        mpCurrentKeyFrame = mlNewKeyFrames.front();
        mlNewKeyFrames.pop_front();
    }
​
    // step2. 计算该关键帧的词向量
    mpCurrentKeyFrame->ComputeBoW();
​
    // step3. 根据地图点中是否观测到当前关键帧判断该地图是是否是新生成的
    const vector<MapPoint *> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for (size_t i = 0; i < vpMapPointMatches.size(); i++) {
        MapPoint *pMP = vpMapPointMatches[i];
        if (pMP && !pMP->isBad()) {
            if (!pMP->IsInKeyFrame(mpCurrentKeyFrame)) {
                // step3.1. 该地图点是跟踪本关键帧时匹配得到的,在地图点中加入对当前关键帧的观测
                pMP->AddObservation(mpCurrentKeyFrame, i);
                pMP->UpdateNormalAndDepth();
                pMP->ComputeDistinctiveDescriptors();
            } else // this can only happen for new stereo points inserted by the Tracking
            {
                // step3.2. 该地图点是跟踪本关键帧时新生成的,将其加入容器mlpRecentAddedMapPoints待筛选
                mlpRecentAddedMapPoints.push_back(pMP);
            }
        }
    }
    
    // step4. 更新共视图关系
    mpCurrentKeyFrame->UpdateConnections();
    
    // step5. 将关键帧插入到地图中
    mpMap->AddKeyFrame(mpCurrentKeyFrame);
}

8.4 剔除坏地图点: MapPointCulling()

冗余地图点的标准:满足以下其中之一就算是坏地图点

1. $$召回率=\frac{实际观测到该地图点的帧数mnFound}{理论上应当观测到该地图点的帧数mnVisible}<0.25$$

2. 在创建的3帧内观测数目少于2(双目为3)

若地图点经过了连续3个关键帧仍未被剔除,则被认为是好的地图点

void LocalMapping::MapPointCulling() {
    list<MapPoint *>::iterator lit = mlpRecentAddedMapPoints.begin();
    const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
​
    int nThObs;
    if (mbMonocular)
        nThObs = 2;
    else
        nThObs = 3;
    const int cnThObs = nThObs;
​
    while (lit != mlpRecentAddedMapPoints.end()) {
        MapPoint *pMP = *lit;
        if (pMP->isBad()) {
            // 标准0: 地图点在其他地方被删除了
            lit = mlpRecentAddedMapPoints.erase(lit);
        } else if (pMP->GetFoundRatio() < 0.25f) {
            // 标准1: 召回率<0.25
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        } else if (((int) nCurrentKFid - (int) pMP->mnFirstKFid) >= 2 && pMP->Observations() <= cnThObs) {
            // 标准2: 从创建开始连续3个关键帧内观测数目少于cnThObs
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        } else if (((int) nCurrentKFid - (int) pMP->mnFirstKFid) >= 3)
            // 通过了3个关键帧的考察,认为是好的地图点
            lit = mlpRecentAddedMapPoints.erase(lit);
        else
            lit++;
    }
}

MapPoint类中关于召回率的成员函数和变量如下:

成员函数/变量	访问控制	意义	初值
int mnFound	protected	实际观测到该地图点的帧数	1
int mnVisible	protected	理论上应当观测到该地图点的帧数	1
float GetFoundRatio()	public	召回率实际观测到该地图点的帧数理论上应当观测到该地图点的帧数
void IncreaseFound(int n=1)	public	mnFound加1
void IncreaseVisible(int n=1)	public	mnVisible加1

这两个成员变量主要用于Tracking线程.

• 在函数Tracking::SearchLocalPoints()中,会对所有处于当前帧视锥内的地图点调用成员函数MapPoint::IncreaseVisible().(这些点未必真的被当前帧观测到了,只是地理位置上处于当前帧视锥范围内).

void Tracking::SearchLocalPoints() {
    // 当前关键帧的地图点
    for (MapPoint *pMP : mCurrentFrame.mvpMapPoints) {
        pMP->IncreaseVisible();
            }
        }
    }
​
    // 局部关键帧中不属于当前帧,但在当前帧视锥范围内的地图点
    for (MapPoint *pMP = *vit : mvpLocalMapPoints.begin()) {
        if (mCurrentFrame.isInFrustum(pMP, 0.5)) {
            pMP->IncreaseVisible();
        }
    }
​
    // ...
}

在函数Tracking::TrackLocalMap()中,会对所有当前帧观测到的地图点调用MaoPoint::IncreaseFound().

bool Tracking::TrackLocalMap() {
    
    // ...
    
    for (int i = 0; i < mCurrentFrame.N; i++) {
        if (mCurrentFrame.mvpMapPoints[i]) {
            if (!mCurrentFrame.mvbOutlier[i]) {
                // 当前帧观测到的地图点
                mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
                // ...
            }
        }
    }
    
    // ...
}

8.5 创建新地图点: CreateNewMapPoints()

将当前关键帧分别与共视程度最高的前10(单目相机取20)个共视关键帧两两进行特征匹配,生成地图点.

对于双目相机的匹配特征点对,可以根据某帧特征点深度恢复地图点,也可以根据两帧间对极几何三角化地图点,这里取视差角最大的方式来生成地图点.

8.6 融合当前关键帧和其共视帧的地图点: SearchInNeighbors()

本函数将当前关键帧与其一级和二级共视关键帧做地图点融合,分两步:

1. 正向融合: 将当前关键帧的地图点融合到各共视关键帧中.
2. 反向融合: 将各共视关键帧的地图点融合到当前关键帧中.

void LocalMapping::SearchInNeighbors() {
    // step1. 取当前关键帧的一级共视关键帧
    const vector<KeyFrame *> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(10);
​
    // step2. 遍历一级关键帧,寻找二级关键帧
    vector<KeyFrame *> vpTargetKFs;
    for (KeyFrame *pKFi : vpNeighKFs) {
        if (pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)
            continue;
        vpTargetKFs.push_back(pKFi);
        pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;
        const vector<KeyFrame *> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
        for (KeyFrame *pKFi2 : vpSecondNeighKFs) {
            if (pKFi2->isBad() || pKFi2->mnFuseTargetForKF == mpCurrentKeyFrame->mnId || pKFi2->mnId == mpCurrentKeyFrame->mnId)
                continue;
            vpTargetKFs.push_back(pKFi2);
        }
    }
​
​
    // step3. 正向融合: 将当前帧的地图点融合到各共视关键帧中
    vector<MapPoint *> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    ORBmatcher matcher;
    for (KeyFrame *pKFi : vpTargetKFs) {
        matcher.Fuse(pKFi, vpMapPointMatches);
    }
​
    // step4. 反向融合: 将各共视关键帧的地图点融合到当前关键帧中
    // step4.1. 取出各共视关键帧的地图点存入vpFuseCandidates
    vector<MapPoint *> vpFuseCandidates;
    for (KeyFrame *pKFi : vpTargetKFs) {
        vector<MapPoint *> vpMapPointsKFi = pKFi->GetMapPointMatches();
        for (MapPoint *pMP : vpMapPointsKFi.begin()) {
            if (!pMP || pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
                continue;
            pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;
            vpFuseCandidates.push_back(pMP);
        }
    }
​
    // step 4.2. 进行反向融合
    matcher.Fuse(mpCurrentKeyFrame, vpFuseCandidates);
​
    // step5. 更新当前关键帧的地图点信息
    vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
    for (MapPoint *pMP : vpMapPointMatches) {
        if (pMP and !pMP->isBad()) {
            pMP->ComputeDistinctiveDescriptors();
            pMP->UpdateNormalAndDepth();
        }
    }
    
    // step6. 更新共视图
    mpCurrentKeyFrame->UpdateConnections();
}

ORBmatcher::Fuse()将地图点与帧中图像的特征点匹配,实现地图点融合.

在将地图点反投影到帧中的过程中,存在以下两种情况:

1. 若地图点反投影对应位置上不存在地图点,则直接添加观测.
2. 若地图点反投影位置上存在对应地图点,则将两个地图点合并到其中观测较多的那个.

int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th) {
    // 遍历所有的待投影地图点
    for(MapPoint* pMP : vpMapPoints) {
        // step1. 将地图点反投影到相机成像平面上
        const float invz = 1/p3Dc.at<float>(2);
        const float x = p3Dc.at<float>(0)*invz;
        const float y = p3Dc.at<float>(1)*invz;
        const float u = fx*x+cx;
        const float v = fy*y+cy;
        const float ur = u-bf*invz;
        const float maxDistance = pMP->GetMaxDistanceInvariance();
        const float minDistance = pMP->GetMinDistanceInvariance();
        cv::Mat PO = p3Dw-Ow;
        const float dist3D = cv::norm(PO);
​
        // step2. 地图点观测距离
        if(dist3D<minDistance || dist3D>maxDistance )
            continue;
​
        // step3. 地图点的观测距离和观测方向不能太离谱
        if (dist3D < minDistance || dist3D > maxDistance)
            continue;
        cv::Mat Pn = pMP->GetNormal();
        if (PO.dot(Pn) < 0.5 * dist3D)
            continue;
​
        // step4. 在投影位置寻找图像特征点
        int nPredictedLevel = pMP->PredictScale(dist3D, pKF);
        const float radius = th * pKF->mvScaleFactors[nPredictedLevel];
        const vector<size_t> vIndices = pKF->GetFeaturesInArea(u, v, radius);
        const cv::Mat dMP = pMP->GetDescriptor();
        int bestDist = 256;
        int bestIdx = -1;
        for (size_t idx : vIndices) {
            const size_t idx = *vit;
            const cv::KeyPoint &kp = pKF->mvKeysUn[idx];
            const int &kpLevel = kp.octave;
            // step4.1. 金字塔层级要接近
            if (kpLevel < nPredictedLevel - 1 || kpLevel > nPredictedLevel)
                continue;
            // step4.2. 使用卡方检验检查重投影误差,单目和双目的自由度不同
            if (pKF->mvuRight[idx] >= 0) {
                const float ex = u - kp.pt.x;
                const float ey = v - kp.pt.y;
                const float er = ur - pKF->mvuRight[idx];
                const float e2 = ex * ex + ey * ey + er * er;
                if (e2 * pKF->mvInvLevelSigma2[kpLevel] > 7.8)
                    continue;
            } else {
                const float ex = u - kp.pt.x;
                const float ey = v - kp.pt.y;
                const float e2 = ex * ex + ey * ey;
                if (e2 * pKF->mvInvLevelSigma2[kpLevel] > 5.99)
                    continue;
            }
            // step4.3. 检验描述子距离
            const cv::Mat &dKF = pKF->mDescriptors.row(idx);
            const int dist = DescriptorDistance(dMP, dKF);
            if (dist < bestDist) {
                bestDist = dist;
                bestIdx = idx;
            }
        }
​
        // step5. 与最近特征点的描述子距离足够小,就进行地图点融合
        if (bestDist <= TH_LOW) {
            MapPoint *pMPinKF = pKF->GetMapPoint(bestIdx);
            if (pMPinKF) {
                // step5.1. 地图点反投影位置上存在对应地图点,则将两个地图点合并到其中观测较多的那个则直接添加观测
                if (!pMPinKF->isBad()) {
                    if (pMPinKF->Observations() > pMP->Observations())
                        pMP->Replace(pMPinKF);
                    else
                        pMPinKF->Replace(pMP);
                }
            } else {
                // step5.2. 地图点反投影对应位置上不存在地图点,
                pMP->AddObservation(pKF, bestIdx);
                pKF->AddMapPoint(pMP, bestIdx);
            }
            nFused++;
        }
    }
​
    return nFused;
}

8.7 局部BA优化: Optimizer::LocalBundleAdjustment()

局部BA优化当前帧的局部地图.

• 当前关键帧的一级共视关键帧位姿会被优化;二极共视关键帧会加入优化图,但其位姿不会被优化.
• 所有局部地图点位姿都会被优化.

Tracking线程中定义了局部地图成员变量mvpLocalKeyFrames和mvpLocalMapPoints,但是这些变量并没有被LocalMapping线程管理,因此在函数Optimizer::LocalBundleAdjustment()中还要重新构造局部地图变量,这种设计有些多此一举了.

8.8 剔除冗余关键帧: KeyFrameCulling()

冗余关键帧标准: 90%以上的地图点能被超过3个其他关键帧观测到.

void LocalMapping::KeyFrameCulling() {
​
    // step1. 遍历当前关键帧的所有共视关键帧
    vector<KeyFrame *> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
    for (KeyFrame *pKF : vpLocalKeyFrames) {
​
        // step2. 遍历所有局部地图点
        const vector<MapPoint *> vpMapPoints = pKF->GetMapPointMatches();
        int nRedundantObservations = 0;
        int nMPs = 0;
        for (MapPoint *pMP : vpMapPoints) {
            if (pMP && !pMP->isBad()) {
                if (!mbMonocular) {
                    // 双目相机只能看到不超过相机基线35倍的地图点
                    if (pKF->mvDepth[i] > pKF->mThDepth || pKF->mvDepth[i] < 0)
                        continue;
                }
                nMPs++;
                
                int nObs = 0;
                for (KeyFrame *pKFi : pMP->GetObservations()) {
                     = mit->first;
                    if (pKFi->mvKeysUn[mit->second].octave <= pKF->mvKeysUn[i].octave + 1) {
                        nObs++;
                        if (nObs >= 3)
                            break;
                    }
                }
                if (nObs >= 3) {
                    nRedundantObservations++;
                }
            }
        }
    }
​
    // step3. 若关键帧超过90%的地图点能被超过3个其它关键帧观测到,则视为冗余关键帧
    if (nRedundantObservations > 0.9 * nMPs)
        pKF->SetBadFlag();
}

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

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

9. ORB-SLAM2代码详解09_闭环线程LoopClosing

9.1 各成员函数/变量

9.1.1 闭环主函数: Run()

void LoopClosing::Run() {
    while (1) {
        if (CheckNewKeyFrames()) {
            if (DetectLoop()) {
                if (ComputeSim3()) {
                    CorrectLoop();
                }
            }
        }
​
        std::this_thread::sleep_for(std::chrono::milliseconds(5));
    }
}

9.2 闭环检测: DetectLoop()

LoopClosing类中定义类型ConsistentGroup,表示关键帧组.

typedef pair<set<KeyFrame *>, int> ConsistentGroup

• 第一个元素表示一组共视关键帧.
• 第二个元素表示该关键帧组的连续长度.

所谓连续,指的是两个关键帧组中存在相同的关键帧.

成员函数/变量	访问控制	意义
KeyFrame *mpCurrentKF	protected	当前关键帧
KeyFrame *mpMatchedKF	protected	当前关键帧的闭环匹配关键帧
std::vector mvConsistentGroups	protected	前一关键帧的闭环候选关键帧组
vCurrentConsistentGroups	局部变量	当前关键帧的闭环候选关键帧组
std::vector mvpEnoughConsistentCandidates	protected	所有达到足够连续数的关键帧

闭环检测原理: 若连续4个关键帧都能在数据库中找到对应的闭环匹配关键帧组,且这些闭环匹配关键帧组间是连续的,则认为实现闭环,

---

具体来说,回环检测过程如下:

1. 找到当前关键帧的闭环候选关键帧vpCandidateKFs:

   闭环候选关键帧取自于与当前关键帧具有相同的BOW向量但不存在直接连接的关键帧.

   

2. 将闭环候选关键帧和其共视关键帧组合成为关键帧组vCurrentConsistentGroups:

   

3. 在当前关键组和之前的连续关键组间寻找连续关系.

   • 若当前关键帧组在之前的连续关键帧组中找到连续关系,则当前的连续关键帧组的连续长度加1.
   • 若当前关键帧组在之前的连续关键帧组中没能找到连续关系,则当前关键帧组的连续长度为0.

   关键帧组的连续关系是指两个关键帧组间是否有关键帧同时存在于两关键帧组中.

   

   若某关键帧组的连续长度达到3,则认为该关键帧实现闭环.

bool LoopClosing::DetectLoop() {
    // step1. 取出缓冲队列头部的关键帧,作为当前检测闭环关键帧,设置其不被优化删除
    {
        unique_lock<mutex> lock(mMutexLoopQueue);
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        mlpLoopKeyFrameQueue.pop_front();
        mpCurrentKF->SetNotErase();
    }
​
    // step2. 距离上次闭环时间太短,不再检测闭环
    if (mpCurrentKF->mnId < mLastLoopKFid + 10) {
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }
​
    // step3. 计算当前关键帧与共视关键帧间最大相似度
    const vector<KeyFrame *> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
    float minScore = 1;
    for (KeyFrame *pKF : vpConnectedKeyFrames) {
        const DBoW2::BowVector &BowVec = pKF->mBowVec;
        float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
        if (score < minScore)
            minScore = score;
    }
​
    // step4. 寻找当前关键帧的闭环候选关键帧
    vector<KeyFrame *> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
    if (vpCandidateKFs.empty()) {
        mpKeyFrameDB->add(mpCurrentKF);
        mvConsistentGroups.clear();
        mpCurrentKF->SetErase();
        return false;
    }
     
    // step5. 在当前关键帧组和之前的连续关键帧组之间寻找匹配
    mvpEnoughConsistentCandidates.clear();
    vector<ConsistentGroup> vCurrentConsistentGroups;
    vector<bool> vbConsistentGroup(mvConsistentGroups.size(), false);   // 之前的连续关键帧组在当前关键帧组中是否存在连续
​
    // 遍历当前闭环候选关键帧
    for (KeyFrame *pCandidateKF : vpCandidateKFs) {
        // step5.1. 构建关键帧组,包括候选关键帧及其共视关键帧
        set<KeyFrame *> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
        spCandidateGroup.insert(pCandidateKF);
​
        // step5.2. 遍历之前的连续关键帧组,寻找连续关系        
        bool bEnoughConsistent = false;
        bool bConsistentForSomeGroup = false;
        for (size_t iG = 0, iendG = mvConsistentGroups.size(); iG < iendG; iG++) {
            set<KeyFrame *> sPreviousGroup = mvConsistentGroups[iG].first;
            bool bConsistent = false;
            // step5.2. 若当前连续关键帧组中某关键帧也在前一帧的候选关键帧组中,则找到了连续关系
            for (KeyFrame * previousKeyFrame : spCandidateGroup.begin()) {
                if (sPreviousGroup.count(previousKeyFrame)) {
                    bConsistent = true;
                    bConsistentForSomeGroup = true;
                    break;
                }
            }
​
            // step5.3. 更新当前关键帧组的连续次数
            if (bConsistent) {
                int nCurrentConsistency = mvConsistentGroups[iG].second + 1;
                if (!vbConsistentGroup[iG]) {
                    ConsistentGroup cg = make_pair(spCandidateGroup, nCurrentConsistency);
                    vCurrentConsistentGroups.push_back(cg);
                    vbConsistentGroup[iG] = true;
                }
                // 若当前关键帧组的连续次数达到3,则完成闭环,将其加入到mvpEnoughConsistentCandidates中
                if (nCurrentConsistency >= mnCovisibilityConsistencyTh && !bEnoughConsistent) {
                    mvpEnoughConsistentCandidates.push_back(pCandidateKF);
                    bEnoughConsistent = true;
                }
            }
        }
        
        // 5.4. 若当前关键帧组在前一关键帧的闭环候选关键帧组中找不到连续关系,则将两虚次数置零
        if (!bConsistentForSomeGroup) {
            ConsistentGroup cg = make_pair(spCandidateGroup, 0);
            vCurrentConsistentGroups.push_back(cg);
        }
    }
    
    // step6. 维护循环变量
    mvConsistentGroups = vCurrentConsistentGroups;      // 更新连续关键帧组
    mpKeyFrameDB->add(mpCurrentKF);                     // 将当前关键帧加入到关键帧数据库中
​
    if (mvpEnoughConsistentCandidates.empty()) {
        mpCurrentKF->SetErase();
        return false;
    } else {
        return true;
    }
}

当前关键帧的闭环候选关键帧取自于与当前关键帧具有相同BOW向量但不直接相连的关键帧.

// 寻找当前关键帧的闭环候选关键帧
vector<KeyFrame *> KeyFrameDatabase::DetectLoopCandidates(KeyFrame *pKF, float minScore) {
    
    // step1. 找出当前关键帧的所有共视关键帧
    set<KeyFrame *> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
​
​
    // step2. 找出所有具有相同BOW但不直接相连的关键帧
    list<KeyFrame *> lKFsSharingWords;      // 存储闭环候选关键帧
    {
        unique_lock<mutex> lock(mMutex);
        // 遍历所有BOW词向量
        for (DBoW2::BowVector vit : pKF) {
            // 遍历所有含有该词向量的关键帧
            for (KeyFrame *pKFi : mvInvertedFile[vit.first]) {
                if (pKFi->mnLoopQuery != pKF->mnId) {
                    pKFi->mnLoopWords = 0;
                    // 若该关键帧与当前关键帧不直接相连,才能作为闭环候选
                    if (!spConnectedKeyFrames.count(pKFi)) {
                        pKFi->mnLoopQuery = pKF->mnId;
                        lKFsSharingWords.push_back(pKFi);
                    }
                }
                pKFi->mnLoopWords++;
            }
        }
    }
​
    // step3. 以最大相似度的0.8倍为阈值筛选筛选候选关键帧
    int maxCommonWords = 0;
    list<pair<float, KeyFrame *> > lScoreAndMatch;
    for (KeyFrame *pKFi : lKFsSharingWords) {
        if (*pKFi->mnLoopWords > maxCommonWords)
            maxCommonWords = *pKFi->mnLoopWords;
    }
    int minCommonWords = maxCommonWords * 0.8f;
    for (KeyFrame *pKFi : lKFsSharingWords) {
        if (pKFi->mnLoopWords > minCommonWords) {
            float si = mpVoc->score(pKF->mBowVec, pKFi->mBowVec);
            pKFi->mLoopScore = si;
            if (si >= minScore)
                lScoreAndMatch.push_back(make_pair(si, pKFi));
        }
    }
​
    // step4. 统计候选关键帧的共视关键帧组的相似度得分
    list<pair<float, KeyFrame *> > lAccScoreAndMatch;
    float bestAccScore = minScore;
    for (list<pair<float, KeyFrame *> >::iterator it : lScoreAndMatch) {
        KeyFrame *pKFi = it->second;
        vector<KeyFrame *> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
        float bestScore = it->first; 
        float accScore = it->first;  
        KeyFrame *pBestKF = pKFi;    
        for (vector<KeyFrame *>::iterator vit = vpNeighs.begin(), vend = vpNeighs.end(); vit != vend; vit++) {
            KeyFrame *pKF2 = *vit;
            if (pKF2->mnLoopQuery == pKF->mnId && pKF2->mnLoopWords > minCommonWords) {
                accScore += pKF2->mLoopScore;
                if (pKF2->mLoopScore > bestScore) {
                    pBestKF = pKF2;
                    bestScore = pKF2->mLoopScore;
                }
            }
        }
​
        lAccScoreAndMatch.push_back(make_pair(accScore, pBestKF));
        if (accScore > bestAccScore)
            bestAccScore = accScore;
    }
​
    // step5. 取相似度得分高于最高相似度0.75的组的最优匹配关键帧    
    float minScoreToRetain = 0.75f * bestAccScore;
    set<KeyFrame *> spAlreadyAddedKF;
    vector<KeyFrame *> vpLoopCandidates;
    for (list<pair<float, KeyFrame *> >::iterator it : lAccScoreAndMatch.begin()) {
        if (it->first > minScoreToRetain) {
            KeyFrame *pKFi = it->second;
            if (!spAlreadyAddedKF.count(pKFi)) {
                vpLoopCandidates.push_back(pKFi);
                spAlreadyAddedKF.insert(pKFi);
            }
        }
    }
​
    return vpLoopCandidates;
}

9.3 计算Sim3变换: ComputeSim3()

成员函数/变量	访问控制	意义
std::vector mvpEnoughConsistentCandidates	protected	在函数LoopClosing::DetectLoop()中找到的有足够连续性的闭环关键帧
g2o::Sim3 mg2oScw cv::Mat mScw	protected protected	世界坐标系w到相机坐标系c的Sim3变换
std::vector mvpLoopMapPoints	protected	闭环关键帧组中的地图点
std::vector mvpCurrentMatchedPoints	protected	当前帧到mvpLoopMapPoints的匹配关系 mvpCurrentMatchedPoints[i]表示当前帧第i个特征点对应的地图点

bool LoopClosing::ComputeSim3() {
    const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
    ORBmatcher matcher(0.75, true);
​
    vector<Sim3Solver *> vpSim3Solvers;                 // 保存每个闭环匹配关键帧的Sim3Solver
    vector<vector<MapPoint *> > vvpMapPointMatches;     // 保存当前关键帧到每个闭环匹配关键帧的匹配关系
    vector<bool> vbDiscarded;                           // 保存每个闭环匹配关键帧是否是误匹配
​
    // step1. 为每个有超过20个匹配点的闭环关键帧创建Sim3Solver
    int nCandidates = 0;
    for (int i = 0; i < nInitialCandidates; i++) {
        KeyFrame *pKF = mvpEnoughConsistentCandidates[i];
        pKF->SetNotErase();
        int nmatches = matcher.SearchByBoW(mpCurrentKF, pKF, vvpMapPointMatches[i]);
        if (nmatches < 20) {
            vbDiscarded[i] = true;
            continue;
        } else {
            Sim3Solver *pSolver = new Sim3Solver(mpCurrentKF, pKF, vvpMapPointMatches[i], mbFixScale);
            pSolver->SetRansacParameters(0.99, 20, 300);
            vpSim3Solvers[i] = pSolver;
        }
        nCandidates++;
    }
​
    // step2. 对每个闭环候选关键帧求解优化Sim3
    bool bMatch = false;        // 是否有帧通过Sim3求解
    while (nCandidates > 0 && !bMatch) {
        for (int i = 0; i < nInitialCandidates; i++) {
            if (vbDiscarded[i])
                continue;
​
            KeyFrame *pKF = mvpEnoughConsistentCandidates[i];
            vector<bool> vbInliers;
            int nInliers;
            bool bNoMore;
            // step2.1. 进行Sim3迭代求解
            Sim3Solver *pSolver = vpSim3Solvers[i];
            cv::Mat Scm = pSolver->iterate(5, bNoMore, vbInliers, nInliers);
            if (bNoMore) {
                vbDiscarded[i] = true;
                nCandidates--;
            }
​
            if (!Scm.empty()) {
                // step2.2 根据计算出的Sim3搜索匹配点
                vector<MapPoint *> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint *>(NULL));
                for (size_t j = 0, jend = vbInliers.size(); j < jend; j++) {
                    if (vbInliers[j])
                        vpMapPointMatches[j] = vvpMapPointMatches[i][j];
                }
                cv::Mat R = pSolver->GetEstimatedRotation();
                cv::Mat t = pSolver->GetEstimatedTranslation();
                const float s = pSolver->GetEstimatedScale();
                matcher.SearchBySim3(mpCurrentKF, pKF, vpMapPointMatches, s, R, t, 7.5);
​
                // step2.3. 根据搜索出的匹配点优化Sim3
                g2o::Sim3 gScm(Converter::toMatrix3d(R), Converter::toVector3d(t), s);
                const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
                if (nInliers >= 20) {
                    bMatch = true;
                    mpMatchedKF = pKF;
                    g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()), Converter::toVector3d(pKF->GetTranslation()), 1.0);
                    mg2oScw = gScm * gSmw;
                    mScw = Converter::toCvMat(mg2oScw);
                    mvpCurrentMatchedPoints = vpMapPointMatches;
                    break;
                }
            }
        }
    }
​
    // step2.4. 优化失败,退出函数
    if (!bMatch) {
        for (int i = 0; i < nInitialCandidates; i++)
            mvpEnoughConsistentCandidates[i]->SetErase();
        mpCurrentKF->SetErase();
        return false;
    }
​
    // step3. 将闭环关键帧及其共视关键帧的所有地图点 投影到 当前关键帧
    vector<KeyFrame *> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
    vpLoopConnectedKFs.push_back(mpMatchedKF);
    for (KeyFrame *pKF : vpLoopConnectedKFs) {
        for (MapPoint *pMP : pKF->GetMapPointMatches()) {
            if (pMP && !pMP->isBad() && pMP->mnLoopPointForKF != mpCurrentKF->mnId) {
                mvpLoopMapPoints.push_back(pMP);
                pMP->mnLoopPointForKF = mpCurrentKF->mnId;
            }
        }
    }
    matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints, 10);
​
    // step5. 根据投影成功的地图点数判断Sim3计算的是否准确
    int nTotalMatches = 0;
    for (size_t i = 0; i < mvpCurrentMatchedPoints.size(); i++) {
        if (mvpCurrentMatchedPoints[i])
            nTotalMatches++;
    }
​
    if (nTotalMatches >= 40) {
        for (int i = 0; i < nInitialCandidates; i++)
            if (mvpEnoughConsistentCandidates[i] != mpMatchedKF)
                mvpEnoughConsistentCandidates[i]->SetErase();
        return true;
    } else {
        for (int i = 0; i < nInitialCandidates; i++)
            mvpEnoughConsistentCandidates[i]->SetErase();
        mpCurrentKF->SetErase();
        return false;
    }
}

9.4 闭环矫正: CorrectLoop()

函数LoopClosing::CorrectLoop()的主要流程:

1. Sim3位姿传播:
   • 将Sim3位姿传播到局部关键帧组上.
   • 将Sim3位姿传播到局部地图点上.
2. 地图点融合:
   • 将闭环关键帧组地图点投影到当前关键帧上.
   • 将闭环关键帧组地图点投影到局部关键帧组上.
3. BA优化
   • 本质图BA优化: 优化所有地图点和关键帧位姿,基于本质图.
   • 全局BA优化: 优化所有地图点和关键帧位姿,基于地图点到关键帧的投影关系.

void LoopClosing::CorrectLoop() {
​
    cout << "Loop detected!" << endl;
​
    // step0. 更新当前关键帧组与地图点的连接
    mpCurrentKF->UpdateConnections();
​
    // step1. Sim3位姿传播
    // step1.1. 构建局部关键帧组
    mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
    mvpCurrentConnectedKFs.push_back(mpCurrentKF);
    map<KeyFrame*, g2o::Sim3> CorrectedSim3, NonCorrectedSim3;  // 存放局部关键帧组Sim3位姿传播前后的位姿
    CorrectedSim3[mpCurrentKF] = mg2oScw;    
    cv::Mat Twc = mpCurrentKF->GetPoseInverse();
    
    {
        unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
​
        // step1.2 将Sim3位姿传播到局部关键帧组中
        for (KeyFrame *pKFi : mvpCurrentConnectedKFs) {
            cv::Mat Tiw = pKFi->GetPose();
            if (pKFi != mpCurrentKF) {
                cv::Mat Tic = Tiw * Twc;
                cv::Mat Ric = Tic.rowRange(0, 3).colRange(0, 3);
                cv::Mat tic = Tic.rowRange(0, 3).col(3);
                g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric), Converter::toVector3d(tic), 1.0);
                g2o::Sim3 g2oCorrectedSiw = g2oSic * mg2oScw;
                CorrectedSim3[pKFi] = g2oCorrectedSiw;
            }
            cv::Mat Riw = Tiw.rowRange(0, 3).colRange(0, 3);
            cv::Mat tiw = Tiw.rowRange(0, 3).col(3);
            g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw), Converter::toVector3d(tiw), 1.0);
            NonCorrectedSim3[pKFi] = g2oSiw;
        }
        
        // step1.3. 将Sim3位姿传播到局部地图点上
        for (pair<KeyFrame*, g2o::Sim3> mit : CorrectedSim3) {
            KeyFrame *pKFi = mit.first;
            g2o::Sim3 g2oCorrectedSiw = mit.second;
            g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
            g2o::Sim3 g2oSiw = NonCorrectedSim3[pKFi];
​
            for (MapPoint *pMPi : pKFi->GetMapPointMatches()) {
                if (!pMPi || pMPi->isBad())
                    continue;
                if (pMPi->mnCorrectedByKF == mpCurrentKF->mnId)     // 标记,防止重复矫正
                    continue;
                cv::Mat P3Dw = pMPi->GetWorldPos();
                Eigen::Matrix<double, 3, 1> eigP3Dw = Converter::toVector3d(P3Dw);
                Eigen::Matrix<double, 3, 1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));
                cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
                pMPi->SetWorldPos(cvCorrectedP3Dw);
                pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
                pMPi->mnCorrectedReference = pKFi->mnId;
                pMPi->UpdateNormalAndDepth();
            }
​
            // 将更新后的Sim3位姿赋值给关键帧变量
            Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
            Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
            double s = g2oCorrectedSiw.scale();
            eigt *= (1. / s);
            cv::Mat correctedTiw = Converter::toCvSE3(eigR, eigt);
            pKFi->SetPose(correctedTiw);
            pKFi->UpdateConnections();
        }
    
        // step2. 地图点融合
        // step2.1 将闭环关键帧组地图点融合到当前关键帧上
        for (size_t i = 0; i < mvpCurrentMatchedPoints.size(); i++) {
            if (mvpCurrentMatchedPoints[i]) {
                MapPoint *pLoopMP = mvpCurrentMatchedPoints[i];
                MapPoint *pCurMP = mpCurrentKF->GetMapPoint(i);
                if (pCurMP)
                    pCurMP->Replace(pLoopMP);       // 闭环关键帧组地图点在地图中的时间更长,位姿更准确
                else {
                    mpCurrentKF->AddMapPoint(pLoopMP, i);
                    pLoopMP->AddObservation(mpCurrentKF, i);
                    pLoopMP->ComputeDistinctiveDescriptors();
                }
            }
        }
​
    }
​
    // step2.2 将闭环关键帧组地图点融合到局部关键帧组上
    SearchAndFuse(CorrectedSim3);
​
​
    // step3. BA优化
    // step3.0. 查找回环连接边,用于和生成树共同组成本质图
    map<KeyFrame *, set<KeyFrame *> > LoopConnections;
    for (KeyFrame *pKFi : mvpCurrentConnectedKFs) {
        vector<KeyFrame *> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
        pKFi->UpdateConnections();
        // 闭环矫正后的共视关系 - 闭环矫正前的共视关系 = 闭环带来的新共视关系
        for (KeyFrame* prev_KFi : vpPreviousNeighbors) {
            LoopConnections[pKFi].erase(*prev_KFi);
        }
        for (KeyFrame* prev_KFi : mvpCurrentConnectedKFs) {
            LoopConnections[pKFi].erase(prev_KFi);
        }
    }
​
    // step3.1. 本质图BA优化
    mpMatchedKF->AddLoopEdge(mpCurrentKF);
    mpCurrentKF->AddLoopEdge(mpMatchedKF);
    Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
​
    // step3.2. 全局BA优化
    mbRunningGBA = true;
    mbFinishedGBA = false;
    mbStopGBA = false;
    mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment, this, mpCurrentKF->mnId);
​
    cout << "Loop Closed!" << endl;
    mLastLoopKFid = mpCurrentKF->mnId;
}

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

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

10. ORB-SLAM2代码详解十大trick

10.1. 关键帧与关键点的删除

由于关键帧与关键点的作用各个地方都要用到，所以对关键帧和地图点的操作就属于一种“重操作”，时间消耗很严重，这里作者采用了很巧妙的办法来处理，以防止系统卡顿或者加锁计算对系统的数据资源的共享。总的来说是采用一种先标记后处理的方式，标志该地图点或者关键帧社会性死亡。当然这带来的副作用就是每一个地图点或者关键帧都多了一个状态值，每次访问之前都需要判断关键帧或者地图点的状态以判断该地图点或者关键帧的状态还正常。

10.2 ORB特征点提取过程中的超像素处理

首先作者通过很巧妙的方法（极大值抑制，分区处理，递归等方法）获取了均匀分布的特征点。随后在极线方向进行搜索，获取对应的匹配点。接着在对应的匹配点的位置左右俩个侧，取三个点，根据极限上三个点的位置拟合出一条高斯曲线，获取你和的高斯曲线的最值所在的点，也即最终的匹配点在极限上的坐标，从而完成了从像素到超像素级别的求解，这个设计很nice!

10.3 最小生成树的维护

10.4 不同高斯金字塔下的视差与距离的约束关系的增加

11. ORB-SLAM2代码详解之十大缺点及待优化空间

rrentKF] = mg2oScw;
cv::Mat Twc = mpCurrentKF->GetPoseInverse();

{
    unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

​
// step1.2 将Sim3位姿传播到局部关键帧组中
for (KeyFrame *pKFi : mvpCurrentConnectedKFs) {
cv::Mat Tiw = pKFi->GetPose();
if (pKFi != mpCurrentKF) {
cv::Mat Tic = Tiw * Twc;
cv::Mat Ric = Tic.rowRange(0, 3).colRange(0, 3);
cv::Mat tic = Tic.rowRange(0, 3).col(3);
g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric), Converter::toVector3d(tic), 1.0);
g2o::Sim3 g2oCorrectedSiw = g2oSic * mg2oScw;
CorrectedSim3[pKFi] = g2oCorrectedSiw;
}
cv::Mat Riw = Tiw.rowRange(0, 3).colRange(0, 3);
cv::Mat tiw = Tiw.rowRange(0, 3).col(3);
g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw), Converter::toVector3d(tiw), 1.0);
NonCorrectedSim3[pKFi] = g2oSiw;
}

    // step1.3. 将Sim3位姿传播到局部地图点上
    for (pair<KeyFrame*, g2o::Sim3> mit : CorrectedSim3) {
        KeyFrame *pKFi = mit.first;
        g2o::Sim3 g2oCorrectedSiw = mit.second;
        g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
        g2o::Sim3 g2oSiw = NonCorrectedSim3[pKFi];

​
for (MapPoint *pMPi : pKFi->GetMapPointMatches()) {
if (!pMPi || pMPi->isBad())
continue;
if (pMPi->mnCorrectedByKF == mpCurrentKF->mnId) // 标记,防止重复矫正
continue;
cv::Mat P3Dw = pMPi->GetWorldPos();
Eigen::Matrix<double, 3, 1> eigP3Dw = Converter::toVector3d(P3Dw);
Eigen::Matrix<double, 3, 1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));
cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
pMPi->SetWorldPos(cvCorrectedP3Dw);
pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
pMPi->mnCorrectedReference = pKFi->mnId;
pMPi->UpdateNormalAndDepth();
}
​
// 将更新后的Sim3位姿赋值给关键帧变量
Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
double s = g2oCorrectedSiw.scale();
eigt *= (1. / s);
cv::Mat correctedTiw = Converter::toCvSE3(eigR, eigt);
pKFi->SetPose(correctedTiw);
pKFi->UpdateConnections();
}

    // step2. 地图点融合
    // step2.1 将闭环关键帧组地图点融合到当前关键帧上
    for (size_t i = 0; i < mvpCurrentMatchedPoints.size(); i++) {
        if (mvpCurrentMatchedPoints[i]) {
            MapPoint *pLoopMP = mvpCurrentMatchedPoints[i];
            MapPoint *pCurMP = mpCurrentKF->GetMapPoint(i);
            if (pCurMP)
                pCurMP->Replace(pLoopMP);       // 闭环关键帧组地图点在地图中的时间更长,位姿更准确
            else {
                mpCurrentKF->AddMapPoint(pLoopMP, i);
                pLoopMP->AddObservation(mpCurrentKF, i);
                pLoopMP->ComputeDistinctiveDescriptors();
            }
        }
    }

​
}
​
// step2.2 将闭环关键帧组地图点融合到局部关键帧组上
SearchAndFuse(CorrectedSim3);
​
​
// step3. BA优化
// step3.0. 查找回环连接边,用于和生成树共同组成本质图
map<KeyFrame *, set<KeyFrame *> > LoopConnections;
for (KeyFrame *pKFi : mvpCurrentConnectedKFs) {
vector<KeyFrame > vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
pKFi->UpdateConnections();
// 闭环矫正后的共视关系 - 闭环矫正前的共视关系 = 闭环带来的新共视关系
for (KeyFrame prev_KFi : vpPreviousNeighbors) {
LoopConnections[pKFi].erase(prev_KFi);
}
for (KeyFrame prev_KFi : mvpCurrentConnectedKFs) {
LoopConnections[pKFi].erase(prev_KFi);
}
}
​
// step3.1. 本质图BA优化
mpMatchedKF->AddLoopEdge(mpCurrentKF);
mpCurrentKF->AddLoopEdge(mpMatchedKF);
Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
​
// step3.2. 全局BA优化
mbRunningGBA = true;
mbFinishedGBA = false;
mbStopGBA = false;
mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment, this, mpCurrentKF->mnId);
​
cout << “Loop Closed!” << endl;
mLastLoopKFid = mpCurrentKF->mnId;
}

[外链图片转存中...(img-1VP3DnGq-1651331512013)]

[外链图片转存中...(img-L79wrR3y-1651331512013)]



# 10. ORB-SLAM2代码详解十大trick

## 10.1. 关键帧与关键点的删除

由于关键帧与关键点的作用各个地方都要用到，所以对关键帧和地图点的操作就属于一种“重操作”，时间消耗很严重，这里作者采用了很巧妙的办法来处理，以防止系统卡顿或者加锁计算对系统的数据资源的共享。总的来说是采用一种先标记后处理的方式，标志该地图点或者关键帧社会性死亡。当然这带来的副作用就是每一个地图点或者关键帧都多了一个状态值，每次访问之前都需要判断关键帧或者地图点的状态以判断该地图点或者关键帧的状态还正常。

## 10.2  ORB特征点提取过程中的超像素处理

首先作者通过很巧妙的方法（极大值抑制，分区处理，递归等方法）获取了均匀分布的特征点。随后在极线方向进行搜索，获取对应的匹配点。接着在对应的匹配点的位置左右俩个侧，取三个点，根据极限上三个点的位置拟合出一条高斯曲线，获取你和的高斯曲线的最值所在的点，也即最终的匹配点在极限上的坐标，从而完成了从像素到超像素级别的求解，这个设计很nice!

## 10.3 Covisibility Graph



（1）Covisibility Graph：共视图，是一个无向有权图（Graph），这个概念最早来自2010的文章[Closing Loops Without Places]。该图中每个顶点就是关键帧，如果两个关键帧有相同的地图点（即它们有共视点），就把这两个顶点连接起来，连接边的权重就是两个关键帧共享的3D点的个数。局部BA优化依赖的就是一个局部的共视图，全局BA优化依赖的就是一个全局的共视图，总之共视图在ORB SLAM2中用得很多。
 共视图的作用
![在这里插入图片描述](https://img-blog.csdnimg.cn/d641aed809e54b76b6f15b2ddcc5fb81.png)

1、跟踪局部地图，扩大搜索范围 Tracking:UpdateLocalKeyFrames（）

2、局部建图里关键帧之间新建地图点 LocalMapping:：CreateNewMapPoints（） LocalMapping:SearchlnNeighbors（）

3、闭环检测、重定位检测 LoopClosing:：DetectLoop（）、LoopClosing:CorrectLoop（） KeyFrameDatabase:：DetectLoopCandidates KeyFrameDatabase:：DetectRelocalizationCandidates

4、优化 Optimizer:：OptimizeEssentialGraph

（2）Essential Graph：为了在优化阶段减小计算量，ORB-SLAM2作者提出了Essential Graph的概念，主要用它来进行全局位姿优化。它是共视图的子集，即Covisibity Graph的最小生成树（MST）。该图中顶点是关键帧，但只连接某个关键帧和与之拥有最多共享的地图点的关键帧，这样能够连接所有的顶点，但是边会减少很多。
本质图（Essential Graph）
![在这里插入图片描述](https://img-blog.csdnimg.cn/5e5a6fac948649a0b6fde108eab2ae33.png)

共视图比较稠密，本质图比共视图更稀疏，这是因为本质图的作用是用在闭环矫正时，用相似变换来矫 正尺度漂移，把闭环误差均摊在本质图中。本质图中节点也是所有关键帧，但是连接边更少，只保留了 联系紧密的边来使得结果更精确。本质图中包含：

1. 扩展树连接关系
2. 形成闭环的连接关系，闭环后地图点变动后新增加的连接关系
3. 共视关系非常好（至少100个共视地图点）的连接关系
本质图优化

```cpp
/Optimizer.cc
Optimizer::OptimizeEssentialGraph()
{
// 省略....
// Spanning tree edge
// Step 4.1：添加扩展树的边（有父关键帧）
// 父关键帧就是和当前帧共视程度最高的关键帧
if(pParentKF)
{
int nIDj = pParentKF->mnId;
g2o::Sim3 Sjw;
LoopClosing::KeyFrameAndPose::const_iterator itj =
NonCorrectedSim3.find(pParentKF);
// 尽可能得到未经过Sim3传播调整的位姿
if(itj!=NonCorrectedSim3.end())
Sjw = itj->second;
else
Sjw = vScw[nIDj];
// 计算父子关键帧之间的相对位姿
g2o::Sim3 Sji = Sjw * Swi;
g2o::EdgeSim3* e = new g2o::EdgeSim3();
本质图优化和全局BA结果对比
从结果来看，
1、全局BA存在收敛问题。即使迭代100次，相对均方误差RMSE 也比较高
2、essential graph 优化可以快速收敛并且结果更精确。θmin 表示被选为essential graph至少需要的
共视地图点数目，从结果来看，θmin的大小对精度影响不大，但是较大的θmin值可以显著减少运行时
间
3、essential graph 优化 后增加全局 full BA 可以提升精度（但比较有限），但是会耗时较多
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertex(nIDj)));
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertex(nIDi)));
// 希望父子关键帧之间的位姿差最小
e->setMeasurement(Sji);
// 所有元素的贡献都一样;每个误差边对总误差的贡献也都相同
e->information() = matLambda;
optimizer.addEdge(e);
}
// 省略....
}

本质图优化和全局BA结果对比 从结果来看，
1、全局BA存在收敛问题。即使迭代100次，相对均方误差RMSE 也比较高

2、essential graph 优化可以快速收敛并且结果更精确。θmin 表示被选为essential graph至少需要的 共视地图点数目，从结果来看，θmin的大小对精度影响不大，但是较大的θmin值可以显著减少运行时间

（3）Spanning Graph：生成树，是代价最小的全联通图，Essential Graph就是基于Spanning Graph生成的。

3、essential graph 优化 后增加全局 full BA 可以提升精度（但比较有限），但是会耗时较多

10.4 关键帧的约束关系的增加

1. 什么是关键帧？
   通俗来说，关键帧就是几帧普通帧里面具有代表性的一帧。

2. 为什么需要关键帧?

1. 相近帧之间信息冗余度很高，关键帧是取局部相近帧中最有代表性的一帧，可以降低信息冗余度。举例来说，摄像头放在原处不动，普通帧还是要记录的，但关键帧不会增加。
2. 关键帧选择时还会对图片质量、特征点质量等进行考察，在Bundle Fusion、RKD SLAM等RGB-D SLAM相关方案中常常用普通帧的深度投影到关键帧上进行深度图优化，一定程度上关键帧是普通帧滤波和优化的结果，防止无用的或错误的信息进入优化过程而破坏定位 建图的准确性。
3. 如果所有帧全部参与计算，不仅浪费了算力，对内存也是极大的考验，这一点在前端vo中表现不明显，但在后端优化里是一个大问题，所以关键帧主要作用是面向后端优化的算力与精度的折中，使得有限的计算资源能够用在刀刃上，保证系统的平稳运行。假如你放松 ORB_SLAM2关键帧选择条件，大量产生的关键帧不仅耗计算资源，还会导致local mapping 计算不过来，出现误差累积

3. 如何选择关键帧？
   选择关键帧主要从关键帧自身和关键帧与其他关键帧的关系2方面来考虑。

1. 关键帧自身质量要好，例如不能是非常模糊的图像、特征点数量要充足、特征点分布要尽量均匀等等；
   关键帧与其他关键帧之间的关系，需要和局部地图中的其他关键帧有一定的共视关系但又不能重复度太高，以达到既存在约束，又尽量
   少的信息冗余的效果。
   选取的指标主要有：

2. 距离上一关键帧的帧数是否足够多（时间）——比如我每隔固定帧数选择一个关键帧，这样编程简单但效果不好。比如运动很慢的时候，就会选择大量相似的关键帧，冗余，运动快的时候又丢失了很多重要的帧。

3. 距离最近关键帧的距离是否足够远（空间）/运动——比如相邻帧根据pose计算运动的相对大小，可以是位移也可以是旋转或者两个都考虑，运动足够大（超过一定阈值）就新建一个关键帧，这种方法比第一种好。但问题是如果对着同一个物体来回扫就会出现大量相似关键帧。

4. 跟踪局部地图质量（共视特征点数目）——记录当前视角下跟踪的特征点数或者比例，当相机离开当前场景时（双目或比例明显降低）才会新建关键帧，避免了第2种方法的问题。缺点是数据结构和逻辑比较复杂。

重点是：引入关键帧“簇”的概念！！！

在关键帧的运用上，我认为orbslam2做的非常好，跟踪线程选择关键帧标准较宽松，局部建图线程再跟据共视冗余度进行剔除，尤其是在回环检测中使用了以关键帧为代表的帧“簇”的概念，回环筛选中有一步将关键帧前后10帧为一组，计算组内总分，以最高分的组的0.75为阈值，滤除一些组，再在剩下的组内各自找最高分的一帧作为备选帧，这个方法非常好地诠释了“关键帧代表局部”的这个理念。

4. 关键帧的类型及更新连接关系
   父子关键帧

//KeyFrame.h 文件中
bool mbFirstConnection; // 是否是第一次生成树
KeyFrame* mpParent; // 当前关键帧的父关键帧 （共视程度最高的）
std::set<KeyFrame*> mspChildrens; // 存储当前关键帧的子关键帧

更新连接关系

//KeyFrame.cc
KeyFrame::UpdateConnections()
{
//省略...
// Step 5 更新生成树的连接
if(mbFirstConnection && mnId!=0)
{
// 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
mpParent = mvpOrderedConnectedKeyFrames.front();
// 建立双向连接关系，将当前关键帧作为其子关键帧
mpParent->AddChild(this);
mbFirstConnection = false;
}
}
// 添加子关键帧（即和子关键帧具有最大共视关系的关键帧就是当前关键帧）
void KeyFrame::AddChild(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
mspChildrens.insert(pKF);
}
// 删除某个子关键帧
void KeyFrame::EraseChild(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
mspChildrens.erase(pKF);
}
// 改变当前关键帧的父关键帧
void KeyFrame::ChangeParent(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
// 添加双向连接关系
mpParent = pKF;
pKF->AddChild(this);
}
//获取当前关键帧的子关键帧
set<KeyFrame*> KeyFrame::GetChilds()
{
unique_lock<mutex> lockCon(mMutexConnections);
return mspChildrens;
}
//获取当前关键帧的父关键帧
KeyFrame* KeyFrame::GetParent()
{
unique_lock<mutex> lockCon(mMutexConnections);
return mpParent;
}
// 判断某个关键帧是否是当前关键帧的子关键帧
bool KeyFrame::hasChild(KeyFrame *pKF)
{
unique_lock<mutex> lockCon(mMutexConnections);
return mspChildrens.count(pKF);
}

更新局部关键帧

void Tracking::UpdateLocalKeyFrames()
{
//省略...
// 策略2.2:将自己的子关键帧作为局部关键帧（将邻居的子孙们拉拢入伙）
const set<KeyFrame*> spChilds = pKF->GetChilds();
for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
{
KeyFrame* pChildKF = *sit;
if(!pChildKF->isBad())
{
if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{
mvpLocalKeyFrames.push_back(pChildKF);
pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
//? 找到一个就直接跳出for循环？
break;
}
}
}
// 策略2.3:自己的父关键帧（将邻居的父母们拉拢入伙）
KeyFrame* pParent = pKF->GetParent();
if(pParent)
{
// mnTrackReferenceForFrame防止重复添加局部关键帧
if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{
mvpLocalKeyFrames.push_back(pParent);
pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
//! 感觉是个bug！如果找到父关键帧会直接跳出整个循环
break;
}
}
// 省略....
}

11. ORB-SLAM2代码详解之十大缺点及待优化空间