ORBSLAM2系统学习（二）

最新推荐文章于 2024-07-24 17:05:18 发布

SYTU---FSQ

最新推荐文章于 2024-07-24 17:05:18 发布

阅读量1.1k

点赞数

文章标签：学习

原文链接：https://blog.csdn.net/weixin_43828675/article/details/115145410

版权

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

前言

理解掌握ORBSLAM2系统整体框架

熟悉此系统ros运行节点线程

一、ORBSLAM2简介

ORB_SLAM2是首个支持单目、双目和RGB-D相机的完整开源SLAM方案，能够实现地图重用，回环检测和重新定位的功能。能够实时计算相机的位姿，并生成场景的稀疏三维重建地图，使用特征点法的巅峰之作，定位精度非常高。

二、系统综述

系统框架

系统主要有三个线程组成：跟踪、Local Mapping（又称小图）、Loop Closing（又称大图）

追踪线程Tracking

这一部分主要工作是从图像中提取ORB特征，根据上一帧进行姿态估计，或者进行通过全局重定位初始化位姿，然后跟踪已经重建的局部地图，优化位姿，再根据一些规则确定新的关键帧。充当视觉里程计的功能。

具体流程：

        1）图像灰度化处理。
        2）构建当前帧（提取每幅图像的特征点，并分配到网格中，这会极大的方便某一领域内的特征点的查找与匹配）。
        3）单目相机初始化操作：通过特征点匹配，使用RANSAC+DLC计算H矩阵，并根据对称转移误差计算H的评分。使用RANSAC+八点法计算E 矩阵，并根据极线约束估计F评分。选择H/E后通过SVD分解获得最优的R/t。随后进行三角化测距，全局BA优化，优化初始地图的相机位姿和路标点坐标。
4）估计相机位姿并进行优化（位姿图优化）。追踪相机位姿的方法（主要的不同在于获得当前帧位姿的方式）包括：(1)恒速运动模型：根据前两帧的位姿变换估计当前帧的位姿，在领域内搜索特征点，随后进行位姿优化。（2）参考关键帧模型：以上一帧的位姿为当前位姿，搜索当前帧的参考关键帧（共视点最多的关键帧），使用词袋加速算法进行特征点匹配，随后优化位姿。（3）重定位模型：使用词袋加速算法获得当前帧的候选关键帧，使用EPNP估计当前帧位姿，进行BA优化。

5）追踪局部地图：将局部地图中的路标点投影到当前帧中，在当前帧中搜索与之匹配的特征点（每个路标点会有一个描述子），随后优化局部地图中的关键帧的位姿。
6）判断关键帧。

局部建图线程local mapping

这一部分主要完成局部地图构建。包括对关键帧的插入，验证最近生成的地图点并进行筛选，然后生成新的地图点，使用Local BA，最后再对插入的关键帧进行筛选，去除多余的关键帧。

具体流程：

        1）插入关键帧：将关键帧插入到局部地图中。在这个过程中会进行一下操作：（1）计算当前关键帧的词袋，并更新关键帧数据库。（2）更新共视图（更新当前帧和与其有共视关系的关键帧间的权重）。
        2）剔除错误的路标点：（1）路标点和关键帧中的特征点匹配，如果匹配成功的关键帧过少，则认为这个路标点是错误的。（2）路标点被3个以下的关键帧观测到。
3）建立新的路标点：寻找与当前帧共视程度最高的候选关键帧，使用词袋加速算法+极限约束寻找特征点匹配。使用三角化测距形成新的路标点。在这个过程中，可能存在特征点1本来与路标点1存在联系，三角化之后，特征点1与路标点2建立了联系。这个时候需要进行路标点的融合。
        4）局部BA优化：优化局部地图中的关键帧位姿和路标点坐标。这里的关键帧指的是与当前帧的共视程度超过一定阈值的关键帧。对于小于阈值的共视关键帧，这提供约束，不会对其位姿进行优化。
        5）删除冗余关键帧：如果一个关键帧的90%以上的路标点可以被3个以上的关键帧观测到，就会被删除。

回环检测loop closing

这一部分主要分为两个过程，分别是闭环探测和闭环校正。闭环检测先使用WOB进行探测，然后通过Sim3算法计算相似变换。闭环校正，主要是闭环融合和Essential Graph的图优化
在局部建图线程中，处理完一个关键帧后会将其抛入回环检测线程中。

具体流程：

        1）检测候选回环关键帧：与当前帧的共视路标点超过15个，且词袋相似度大于一个阈值的关键帧视为候选回环关键帧。
        2）计算Sim3变换：由于单目相机存在尺度漂移现象，因此需要计算当前关键帧相对于回环关键帧以及世界坐标系的Sim3变换。
        3）回环融合：根据当前帧相对于世界坐标的Sim3变换，优化当前帧的共视关键帧的位姿（这样就会减少误差传递过程，使得位姿估计准确）。
        4）本质图优化：对本质图中的关键帧的位姿进行优化。
        5）在回环检测之后还会有一个全局BA优化线程，对所有关键帧的位姿以及路标点的坐标进行优化。
        更多内容参考ORB_SLAM2概述_嚣张的叉烧包的博客-CSDN博客_orb-slam2

三、多线程运转

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

局部建图和回环检测线程都是靠Tracking线程产生的关键帧而运转的，在Tracking线程没有产生关键帧时，局部建图和回环检测线程都是处于空转状态，直到Tracking线程产生关键帧时，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));
    }
}

当三个线程同时运转程序发生混乱的时候，需要给他们加锁。所谓加锁就是只有拿到锁的线程才可以正常运转，运转结束之后自动释放锁。如果一个线程要运转但是锁被其他线程占用着，那只能一直等到其他线程释放锁之后拿到锁才可以继续运行。

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