不那么乐的世界

ORB_SLAM解析

 Tech  3535  8 分钟

ORB_slam解析

目录结构

ORB slam目录中:

  • src为源代码
  • include为源码对应的头文件
  • Example中提供了单目/多目/深度相机及ROS的例子
  • Thirdparty提供了词袋模型和g2O优化库
  • Vocabulary提供了词袋bin文件生成调用方法

ORB slam中的src包括:

  • Converter.cpp 涉及到eigen和opencv矩阵库的转换
  • Frame.cpp 数据帧的方法
  • FrameDrawer.cpp 真画图方法
  • Initializer.cpp 初始化方法
  • KeyFrame.cpp 关键帧方法
  • KeyFrameDataset.cpp 关键帧数据库
  • LocalMapping.cpp 局部地图方法
  • LoopClosing.cpp 闭环监测方法
  • Map.cpp Map中MapPoint和Kframe的操作方法
  • MapDrawer.cpp Map画图
  • MapPoint.cpp MapPoint方法
  • Optimizer.cpp 优化方法
  • ORBextractor.cpp 各类提取器
  • ORBmatcher.cpp 特征点匹配方法
  • PnPsolver.cpp PnP计算方法
  • Sim3Solver.cpp SIM3计算方法
  • System.cpp 代码主流程
  • Tracking.cpp 跟踪主函数的实现
  • Viewer.cpp 画图主函数的实现

Example/Monocular/mono_kitti.cc

main 传递四个参数 可执行文件/词袋模型/KITTI相机配置/图像路径

LoadImages利用图像路径 图像名称 图像时间戳展开 其中后两个类型均为 读取后的数组

LoadImage通过获取time.txt /image_0/ 利用vstrImageFilenames通过循环遍历所有图片

建立ORB_SLAM系统 传递 Vocabulary/ORBvoc.txt词袋模型 Examples/Monocular/KITTIX.yaml相机配置 相机类型 bUseViewer(true)

获取图片数量 然后 循环读取图片和时间戳 im tframe 如果读取图片失败 则返回

SLAM.TrackMonocular(im,tframe); 传入图和时间戳 更新运行时间 直到image图片全部传入 完成后退出

关闭SLAM

统计vTimesTrack 计算中位数时间和平均时间 median time和mean time

核心在于当:

  1. ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); 建立 SLAM系统过程
  2. SLAM.TrackMonocular(im,tframe); 执行TrackMonocular 过程

建立过程

当建立System时 构造需要的参数中:

  1. mbReset
  2. mbActivateLocalizationMode
  3. mbDeactivateLocalizationMode

Reset是用来重新系统 后两个是用户在界面中勾选 未选中则是 false

输出欢迎界面和传感器类型

读取词袋模型 其中实例词袋模型mpVocabulary时的类ORBVocabulary即为 /include/ORBVocabulary.h中 typedef为DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB>,

读取词袋模型 (应该是词袋模型有.txt和.bin两种类型 利用不同的方法读取 方法的实现在/Vocabulary/bin_vocabulary.cpp中)

实例化 mpKeyFrameDatabase 词袋实例

实例化 mpMap 用于地图构建

**实例化 帧画图 **

实例化 地图画图 传入相机配置和地图构建(mpMap strSettingsFile)

实例化 Tracker对象 实例化对象时,构造函数确定相机参数 /畸变矩阵 /帧数/ 更关键的是 ORB参数: 特征点数量 2000/ 比例层级 8 / 比例倍数 1.2 / fIniThFAST(特征点阈值 20)/fMinThFAST(最小阈值 8)

mpORBextractorLeft 实例化特征提取器 (参数就是ORBextractor读取出的参数)

双目和单目分别用不同的 实例化的对象提取器不同 mpORBextractorRight/mpIniORBextractor 不同之处是 单目相机需要更多的 Features

如果是 双目相机或者RGBD相机 还需要设置 远近阈值(ThDepth) 和 DepthMapFactor

实例化 局部地图 实例化局部地图时,传入(mpMap和mSensor==MONOCULAR 也就是True) 实例化完成后 bMonocular= True ( be monocular) 开启线程mptLoopClosing 执行Run方法 Run方法: mbFinished 标志位致为 未完成 进入While(1)循环 跳出条件是 CheckFinish()函数 也就是mbFinishRequested的值为1 跳出 循环中执行:

mbAcceptKeyFrames决定是否接受新的帧

等待处理的关键帧列表不为空 CheckNewKeyFrames()

计算关键帧的Bow映射 关键帧联合地图 ProcessNewKeyFrame()

剔除相邻的MapPoint MapPointCulling()

通过数学原理恢复重建一些MapPoint CreateNewMapPoints

如果 list中还有KeyFrame 融合相邻关键帧中的MapPoints SearchInNeighbors()

--mbAbortBA 是否进行BA优化的标志位 

--队列中为空 并且 闭环检测没有要求停止LocalMapping 利用BA优化器对当前帧进行优化 

--剔除冗余关键帧 :剔除关键帧的90%的MapPoints可以被其他关键帧观测到 --KeyFrameCulling()

--将当前帧加入到闭关检测队列

如果 收到停止指令 --while等待标志位更新 线程休眠3秒后 收到退出请求后 退出 --清空队列和重启请求

然后 setFinish() 退出 RUN() 方法

//103行 实例化 闭环检测 传入 mpMap 关键帧数据库 词袋模型 传感器是否为单目 (这决定了后面的优化类型选择) 开启mptLoopClosing线程 执行LoopClosing run方法 Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的

检测回环 取出列表中的关键帧 然后再删除列表中的该帧 (list.front 获取第一个对象 list.pop_front 删除第一个对象)

设置当前关键帧不可删除 避免被其他线程修改 如果闭环距离上次闭环不到10帧 直接返回 false

超过10帧之后 获取该关键帧连接的关键帧 返回的mvpOrderedConnectedKeyFrames说明已经按照权值排列 获取该关键帧的词袋向量

遍历最小的minScore 利用DetectLoopCandidates找出可能闭环的帧 如果没有可能闭环的帧 就清除变量 返回 false

在候选帧中检测具有连续性的候选帧 构建连续帧组 spCandidateGroup 判定帧组的连续 bConsistent

闭环候选帧 mvpEnoughConsistentCandidates 传入 pCandidateKF 得到的

如果有数据就返回true 为空 则返回 false 置位mpCurrentKF(开锁)

注意 这个ComputeSim3() 是LoopClosing::ComputeSim3 不是solver3sim的

创建一个比较器 matcher 创建一个队列的 sim3Solver求解器 resize到回环候选帧的尺寸

创建一个二维vector存放MapPointMatches

创建一个丢弃标志列

遍历mvpEnoughConsistentCandidates 如果是isbad 设置丢弃

然后进行SearchByBoW 传入当前帧和回环检测帧 MapPoint匹配 通过bow和特征点MapPoint进行快速匹配 将pKF的特征更新到F上去 通过阈值 角度(直方图最大3个index) 和距离投票 返回匹配点的数量

如果匹配点太少 删除该帧 else

创建一个求解器 传入:当前关键帧 回环候选帧 MapPointMatches 单目 然后利用Ransac算法 迭代300次

中间的 continue 结束这次循环

将得到候选帧增加 nCandidates

遍历 nCandidates 遍历每个 nInitialCandidates

剔除vbDiscarded中的序列

Ransac求解mvX3Dc1和mvX3Dc2之间Sim3，函数返回mvX3Dc2到mvX3Dc1的Sim3变换

如果sim3存在 获取[sR t;0 1]

利用 SearchBySim3 更新匹配vpMapPointMatches

优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm

如果优化成功 说明 mpMatchedKF就是最终闭环检测出来与当前帧形成闭环的关键帧

得到从世界坐标系到该候选帧的Sim3变换 gSmw

得到g2o优化后从世界坐标系到当前帧的Sim3变换 mg2oScw

将变换矩阵转成opencv的cv::Mat

这里的break 是退出while循环 bMath已经是true

如果遍历结束后 没有更新bMath 说明 没有闭环检测的候选帧完成了优化

获取关键帧的连接帧 mvpOrderedConnectedKeyFrames

匹配闭环关键帧的连接帧 是否为闭环关键帧

进行一次SearchByProjection搜索一

将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配

判断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配 TotalMatches>40

实例化 mpViewer

获取fps 相机信息 视角点等信息

如果 bUseViewer 开启mptViewer线程 运行Run方法

Pangolin是对OpenGL进行封装的轻量级的OpenGL输入/输出和视频显示的库

pangolin创建windows

设置gl深度

设置 OpenGL使用混合颜色和混合选项

创建按钮和选择框

新建按钮和选择框，第一个参数为按钮的名字，第二个为默认状态，第三个为是否有选择框

定义相机投影模型：ProjectionMatrix

定义观测方位向量：观测点位置

定义面板大小 CreateDisplay

主循环中 利用条件 调整 s_cam和d_cam

利用mpMapDrawer绘制地图

至此 所有建立过程以及完成

最后同步了关联边的信息传递

mpTracker->SetViewer(mpViewer);
//Set pointers between threads
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);

mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);

mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);

执行过程

单目摄像头的执行过程

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
{
    if(mSensor!=MONOCULAR)
    {
        cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;
        exit(-1);
    }

    // Check mode change
    {
        unique_lock<mutex> lock(mMutexMode);
        if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped())
            {
                //usleep(1000);
                std::this_thread::sleep_for(std::chrono::milliseconds(1));
            }

            mpTracker->InformOnlyTracking(true);// 定位时，只跟踪
            mbActivateLocalizationMode = false;// 防止重复执行
        }
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            mbDeactivateLocalizationMode = false;// 防止重复执行
        }
    }

    // Check reset
    {
        unique_lock<mutex> lock(mMutexReset);
        if(mbReset)
        {
            mpTracker->Reset();
            mbReset = false;
        }
    }

    return mpTracker->GrabImageMonocular(im,timestamp);
}

判断传感器是否为单目

判定是否需要启动局部地图 mbActivateLocalizationMode

如果检测到用户勾选改变 发送停止指令

检测直到 mpLocalMapper获取到isStoped状态 线程sleep

定位时,只跟踪 InformOnlyTracking=TRUE /mbOnlyTracking

如果检测到关闭定位 释放局部地图

重启 mbReset

返回 mpTracker->GrabImageMonocular(im,timestamp);

Frame.c & Frame.h

重点理解的变量

  • cv::Mat mTcw 相机姿态 世界坐标系到相机坐标系的变换矩阵
  • cv::Mat Tcw  相机姿态
  • cv::Mat mK 标定矩阵
  • cv::Mat mDistCoef 失真矩阵参数
updatedupdated2020-07-022020-07-02

相关文章：