ORB_SLAM2 简介（一）

ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras
论文下载地址
详解源码https://github.com/electech6/ORB_SLAM2_detailed_comments

一、ORB_SLAM2简介

1、ORB_SLAM2是一个支持单目、双目、RGB-D相机的完整开源SLAM框架
2、能够实现地图重用，回环检测和重新定位的功能
3、后端采用基于单目和双目的光束法平差优化（BA）
4、轻量级的定位模式：在允许零点漂移的条件下，利用视觉里程计追踪未建图的区域并匹配特征点
适用于初学者的第一个开源算法。

二、开源框架

SLAM技术：在未知的环境当中建立一个地图并且能够在地图中实时的定位。ORB_SLAM2方法使用深度信息去合成一个三维坐标，能够精确的提取图像信息。

• ORB_SLAM2文章的系统框架和模块框架
  

三种输入模式：单目模式、双目模式、RGB-D模式

预处理：单目不用操作，双目需要从左右图像中提取ORB特征点，做立体匹配，然后去矫正。RGB-D的图像和深度图需要做一个对齐的操作，这个需要提前标定好内参。

• ORB_SLAM2由跟踪、局部建图和回环检测三个并行的线程和BA优化线程组成
  1、跟踪(Tracking)
  2、建图(LocalMapping)
  3、回环检测(LoopClosing)
  4、BA优化

1、跟踪(Tracking)

初始化初始化成功后首先会选择参考关键帧跟踪，然后大部分时间都是恒速模型跟踪，当跟踪丢失的时候启动重定位跟踪，在经过以上跟踪后可以估计初步的位姿，然后经过局部地图跟踪对位姿进行进一步优化。同时会根据条件判断是否需要将当前帧新建为关键帧。

2、建图(LocalMapping)

输入的关键帧来自跟踪里新建的关键帧。为了增加局部地图点数目，局部地图里关键帧之间会重新进行特征匹配，生成新的地图点， 局部 BA 会同时优化共视图里的关键帧位姿和地图点，优化剔除冗余的关键帧。

3、回环检测(LoopClosing)

这一部分有两步：一个回环信息被确定检测到，另一个是利用这个回环纠正和优化位姿图。闭环检测先进行探测，再通过Sim3算法计算相似变换，然后执行闭环矫正和本质图优化，使得所有关键帧位姿更准确。

4、BA优化

（1）纯运动BA：优化相机旋转矩阵和位置，最小化世界坐标系下匹配3D点云和特征点的重投影误差。
（2）局部BA ：采用一系列可用的关键帧和所有在关键帧可观点，在优化中是固定的。
（3）全局BA：是局部光束法的一个特例，除了初始帧所有的关键帧和点在地图当中都会被优化，用来消除随机化。

三、ORB_SLAM2变量命名规则

• 变量名的第一个字母为m（member的首字母）表示该变量为某类的成员变量
• 变量名的第一、二个字母表示数据类型

p	指针类型
n	int类型
b	bool类型
s	std::set类型
v	std::vector类型
l	std::list类型
KF	KeyFrame类型

（1）mp开头的变量表示指针（pointer）型类成员变量；

Tracking* mpTracker;

（2）mb开头的变量表示布尔（bool）型类成员变量；

bool mbOnlyTracking;

（3）mv开头的变量表示向量（vector）型类成员变量；

std::vector<int> mvIniLastMatches;

（4）mpt开头的变量表示指针（pointer）型类成员变量，并且它是一个线程（thread）；

std::thread* mptLocalMapping;

（5）ml开头的变量表示列表（list）型类成员变量；

list<cv::Mat> mlRelativeFramePoses;

（6）mlp开头的变量表示列表（list）型类成员变量，并且它的元素类型是指针（pointer）；

list<KeyFrame*> mlpReferences;

（7）mlb开头的变量表示列表（list）型类成员变量，并且它的元素类型是布尔型（bool）:

list<bool> mlbLost;

四、多线程

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、线程运行顺序随机

LocalMapping和LoopClosing线程在没有接收到Tracking关键帧时处于空转状态，直到Tracking线程产生了关键帧，3个线程同时运行

// Tracking线程主函数
void Tracking::Track() {
    // 进行跟踪
    // ...
    
    // 若跟踪成功,根据条件判定是否产生关键帧
    if (NeedNewKeyFrame())
        // 产生关键帧并将关键帧传给LocalMapping线程
        KeyFrame *pKF = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB);
        mpLocalMapper->InsertKeyFrame(pKF); 
}

3、线程中的锁

加锁为防止多线程操作同一变量发生混乱，如果某个线程执行到需要锁的位置，直到其他线程释放了这个锁才可以继续执行

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

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

五、ORB_SLAM2优缺点

1、优点

• 支持单目，双目和 RGB-D 相机的完整开源 SLAM 方案，能够实现地图重用、回环检测和重新定位的功能。
• 能够实时计算出相机的位姿，并生成场景的稀疏三维重建地图。
• 支持轻量级定位模式，可以达到零漂移，此时不使用局部建图和回环检测的线程，可以用视觉里程计来跟踪未建图区域。
• 采用 ORB 特征，具有旋转不变性、光照不变性、尺度不变性，匹配速度快，适合实时应用。
• 能够在 CPU 上进行实时工作，可以用于移动终端如 移动机器人、手机、无人机、汽车。
• 跟踪、局部地图、闭环、重定位等所有的任务都采用相同的 ORB 特征，使得系统内数据交互更高效、稳定可靠。
• 单目初始化和应用场景解耦，不管是平面还是非平面场景，都可以自动初始化，无需人工干预。
• 双目和 RGB-D 相对单目相机的主要优势在于，可以直接获得深度信息，不需要像单目情况中那样做一个特定的 SFM 初始化。
• 地图点和关键帧创建比较宽松，但后续会进行严格筛选，剔除冗余关键帧和误差大的地图点，增加建图过程的弹性，在大旋转、快速运动、纹理不足等恶劣情况下可以提高跟踪的鲁棒性。
• 采用共视图，使得跟踪和建图控制在局部共视区域，与全局地图大小无关，可以在大场景下运行。
• 使用本质图（ Essential Graph）来优化位姿实现回环检测，耗时少精度高 。
• 相比于直接法，可以用于宽基线特征匹配，更适合于对深度精度要求较高的场景，比如三维重建。
• 定位精度高，可达厘米级，是特征点法 SLAM 的经典代表作品。
• 代码可读性强，包含很多工程化技巧，非常实用。

2、缺点

• 相比于直接法 SLAM 框架，特征提取部分比较耗时，运行速度没有直接法快。
• 相比于直接法 SLAM 框架，在弱纹理、重复纹理、图像模糊的场景下容易跟踪丢失。
• 产生的定位地图比较稀疏，应用有限。

六、ORB_SLAM2源码框架

ORB_SLAM2算法组成如下：