导图社区 ORB-SLAM3程序思维导图
详细记录了ORB-SLAM3的3个线程的结构。开始时,系统读取一张图像,并将其尺寸调整为合适的大小(resize)。如果这是第一张图像(Ni==0),系统将把当前帧与上一帧之间的所有IMU数据存储在队列vlmuMeas中。在单目模式下,IMU数据被存储在另一个队列mlQueL中,用于后续的处理。整个跟踪过程可能分为多个阶段,包括IMU初始化、相机位姿估计、BA(Bundle Adjustment)优化等。在LocalMapping阶段,系统会处理待处理的关键帧列表。如果列表不为空且IMU数据正常,系统将对这些关键帧进行处理,包括计算BoW(词袋模型)、更新观测、描述子、共视图等,并将它们插入到地图中。
编辑于2024-06-02 22:27:42mono_euroc.cc
LoadImages() (从数据集中加载所有图像的名称和 timestamp)
创建 ORB_SLAM3::System 对象
加载 ORB Vocabulary
创建 KeyFrame Database
创建 Map
创建 FrameDrawer 和 MapDrawer
创建 Tracking 对象
创建 LocalMapping 对象及线程
LocalMapping::Run()
创建 LoopClosing 对象及线程
LoopClosing::Run()
main loop
读入一帧图像
System::TrackMonocular()
Tracking::GrabImageMonocular(im)
转化为灰度图
Frame (创建 Frame 对象)
提取 ORB 特征 (如果是初始化时的帧,则提取两倍特征点数)
Tracking::Track()
System::Shutdown()
System::SaveKeyFrameTrajectoryTUM() (将计算得到的数据集中 KF 的轨迹存储下来)
标记说明
加粗字体:表示程序主框架
加粗字体下的(非加粗字体):表示对加粗字体及其子内容的解释
(非加粗字体):表示对其子内容的概括性解释
带颜色的内容:表示与 ORB-SLAM 论文中系统概览图相对应的每个部分
简称说明
KF:表示 KeyFrame
Current KF:表示当前 KeyFrame
Covisible KF:表示在 Covisibility Graph 中与 Current KF 相连的 KFs
Loop KF:表示回环 KeyFrame,即和 Current KF 相匹配的 KeyFrame Database 中的 KF
Candidate KF:表示可能是 Current KF 的 Loop KF 的候选 KF
Reference KF:表示距离当前帧最近的上一 KF
KF Database:表示用来存储 KFs 的 Database,这些 KFs 会用于回环检测和重定位
LocalMapping 线程
LocalMapping::Run()
while(1)
(插入 KF)
LocalMapping::SetAcceptKeyFrames(false) (通知 Tracking 线程,LocalMapping 线程正忙)
LocalMapping::CheckNewKeyFrames()
if(LocalMapping::CheckNewKeyFrames()) (New KFs 队列中 有新送入且未处理的 KF)
LocalMapping::ProcessNewKeyFrame()
取出 New KFs 队首元素
KeyFrame::ComputeBoW()
更新 MapPoints 与 KeyFrame 的关联
KeyFrame::UpdateConnections()
Map:::AddKeyFrame()
(剔除质量较差的 MapPoints)
LocalMapping::MapPointCulling() (剔除标准可见论文)
(在 Map 中生成新的 MapPoints)
LocalMapping::CreateNewMapPoints() (通过三角化,在Map 中生成新的 MapPoints)
if (LocalMapping::CheckNewKeyFrames() == false) (如果所有新送入的 KFs 都经过上述处理了)
LocalMapping::SearchInNeighbors() (检查 Current KF 与其 Covisible KFs 是否有重复构建的 MapPoints,并进行融合)
找到 Current KF 的 Covisible KFs
for (每一个 Covisible KFs)
ORBmatcher::Fuse(Covisible KF, Current KF 的 MapPointMatches )
ORBmatcher::Fuse(当前 KF, 所有相邻 KFs 的有效 MapPointMatches)
更新 MapPoints 的信息
MapPoint::ComputeDistinctiveDescriptors()
MapPoint::UpdateNormalAndDepth()
if (LocalMapping::CheckNewKeyFrames() == false && 其他线程没有让 LocalMapping 线程暂停)
(Local BA)
Optimizer::LocalBundleAdjustment()
(剔除冗余的 KFs)
LocalMapping::KeyFrameCulling() (剔除标准可见论文)
LoopClosing::InsertKeyFrame() (筛选后的 KFs 送入 LoopClosing 线程)
LocalMapping::SetAcceptKeyFrames(true)
Tracking 线程
Tracking::Track()
if(mState) == NO_IMAGES_YET (读入第一帧图像时)
mSate = NOT_INITIALIZED (去做初始化)
if(mState) (mState 是当前(上一帧)的追踪状态)
== NOT_INITIALIZED
(初始化)
MonocularInitialization() (单目的初始化程序)
第一有效帧设为 Initial Frame 创建 Initializer 对象
第二有效帧设为 Current Frame
ORBmatcher::SearchForInitialization() (Initial Frame 与 Current Frame 之间做特征点匹配)
if (如果匹配特征点对数 < 100)
放弃当前的Initial Frame 和 Current Frame,等待新帧进行初始化
Initializer::Initialize()
两个线程同时计算 (同时计算 IF 和 CF 之间的 Fundmental Matrix 和 Homography)
Initializer::FindHomography()
Initializer::FindFundamental()
if(RH = SH/(SH+SF)) (计算使用两种不同模型的重投影得分,哪个高,就用哪个模型计算出 R 和 t)
> 0.4
ReconstructH()
< 0.4
ReconstructF()
Tracking::CreateInitialMapMonocular() (以 Initial Frame 作为地图坐标原点,创建初始的单目地图,地图初始化)
(将 Initial Frame 和 Current Frame 作为 KF 插入 Map 中)
创建 KeyFrame 对象
KeyFrame::ComputeBoW()
Map::AddKeyFrame()
for (每一个有效匹配点) (根据两个 KFs 匹配的特征点生成 MapPoints)
创建 MapPoint 对象
KeyFrame::AddMapPoint()
MapPoint::AddObservation()
MapPoint::ComputeDistinctiveDescriptors()
MapPoint::UpdateNormalAndDepth()
Map::AddMapPoint()
KeyFrame::UpdateConnections()
Optimizer::GlobalBundleAdjustmnt() (使用一次 Global BA 来对 KFs 位姿和 MapPoints 坐标进行优化)
对 points depth和 t 的尺度进行归一化
LocalMapping::InsertKeyFrame()
Map::SetReferenceMapPoints()
设置 mState = OK
== OK or LOST (系统已经过初始化,进入常规运行)
(相机位姿初值跟踪估计) (根据运动模型或上一 Reference Frame 计算出相机位姿的初值, 便于在局部地图跟踪中进行进一步优化)
if (mbOnlyTracking) (SLAM 模式 / Localization 模式)
== SLAM 模式
if(mSate)
== OK
Tracking::CheckReplacedInLastFrame()
if(运动模型还未建立 or 刚刚进行了重定位)
== true
Tracking::TrackReferenceKeyFrame() (根据 Reference KF 进行估计)
Frame::ComputeBoW()
ORBmatcher::SearchByBoW()
Optimizer::PoseOptimization()
== false
Tracking::TrackWithMotionMode() (根据运动模型(速度不变模型)进行估计)
当前帧 -> SetPose(上一帧位姿变化量 * 上一帧位姿)
ORBmatcher::SearchByProjection()
Optimizer::PoseOptimization()
if (追踪失败)
Tracking::TrackReferenceKeyFrame()
== LOST
Tracking::Relocalization() (Tracking LOST 后,需要进行重定位,将当前帧与 KeyFrameDatabase 中的 KF 进行匹配)
Frame::ComputeBoW()
KeyFrameDatabase::DetectRelocalizationCandidates(当前帧)
for (每一个 Candidate KF) (使用 BoW 匹配初步筛选出一些有足够匹配 words 的 Candidate KFs)
ORBmatcher::SearchByBoW()
筛选
创建 PnPsolver 对象,并设置参数
while (RANSAC 迭代) (通过 RANSAC 迭代求解并匹配,找到最合适的 Candidate KF)
for (每一个筛选后的 Candidate KF)
PnPsolver::iterate() (少量迭代)
nGood = Optimizer::PoseOptimization()
if(nGood)
< 10
continue (不会确定该 Candidate KF 为匹配关键帧)
10 < nGood < 50
ORBmatcher::SearchByProjection()
if(上一步匹配后成功匹配对 > 50)
nGood = Optimizer::PoseOptimization()
if(nGood > 30 & < 50)
ORBmatcher::SearchByProjection()
if(上一步匹配后成功匹配对 > 50)
nGood = Optimizer::PoseOptimization()
if(nGood) >= 50
mnLastRelocFrameId = mCurrentFrame.mnId (将当前帧设为 Tracking 线程中上一次进行 Relocalization 的帧)
break
根据上述步骤的追踪结果设置 bOK 为 OK 还是 LOST (bOK 为当前帧 暂时的 追踪情况)
== Localization 模式 (此时 LocalMapping 线程不工作,即不插入关键帧)
if(mSate)
== OK
if(mbVO) (VO 模式 / 正常模式,VO 模式表示上一帧追踪到的大部分是 VO 点)
== 正常模式
if(运动模型已建立)
== false
Tracking::TrackReferenceKeyFrame()
== true
Tracking::TrackWithMotionMode()
== VO 模式
if(运动模型已建立)
== true
Tracking::TrackWithMotionMode()
Trackng::Relocalization()
if(TrackWithMotionMode() 成功, 同时 Relocalization() 失败)
使用 TrackWithMotionMode() 的结果
if(Relocalization()成功)
使用 Relocalization() 的结果
mbVO = false (从 VO 模式切换为正常模式)
== LOST
Tracking::ReLocalization()
(局部地图跟踪)
if (mbOnlyTracking)
== SLAM 模式
if(当前帧 Tracking OK)
Tracking::TrackLocalMap()
Tracking::UpdateLocalMap()
Tracking::UpdateLocalKeyFrames()
Tracking::UpdateLocalPoints()
Tracking::SearchLocalPoints()
Optimizer::PoseOptimization()
判断 tracking 是否成功
== Localization 模式
if(当前帧 Tracking OK && mbVO == false ) (VO 模式没有局部地图)
Tracking::TrackLocalMap()
根据上述步骤的追踪结果设置 bOK 为 OK 还是 LOST
根据 bOK 设置 mState 为 OK 还是 LOST
FrameDrawer::Update()
(决定是否生成关键帧,并插入关键帧)
if(bOK)
== LOST
if (Map 中的 KeyFrame 少于5帧) (如果系统才刚初始化就 LOST 了,重新初始化吧)
System::Reset()
== OK
更新 运动模型
MapDrawer::SetCurrentCameraPose()
清除 VO 生成的临时性 MapPoints
if (Tracking::NeedNewKeyFrame()) (SLAM模式下才会有 LocalMapping 线程,另外其他判断标准可见论文)
Tracking::CreateNewKeyFrame()
LocalMapping::InsertKeyFrame()
存储计算得到的相机位姿等各种信息
ORB—SLAM3
SLAM::GetImageScale---->Tracking::GetImageScale
main::loop
读入一帧图片,并且resize
Ni==0
Ni>0
把当前帧与上一帧之间的所有IMU数据入栈 vImuMeas.push_back
System::TrackMonocular
确保单目运行
图片risize
单目模式下把IMU数据存储到队列mlQueueImuData
mpTracker->GrabImuData(vImuMeas[i_imu])
计算相机位姿
Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp, string filename)
当前帧转为灰度图像
构造Fram类
mSensor == System::IMU_MONOCULAR
mState==NOT_INITIALIZED || mState==NO_IMAGES_YET
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib)
跟踪
如果上一帧局部建图认为IMU数据有问题,则重置当前活跃地图
mpLocalMapper->mbBadImu
mState!=NO_IMAGES_YET处理时间戳异常
上一帧时戳>当前帧(错误)
重置地图,返回
当前帧>上一帧时戳+1(跳变)
IMU补偿
上一帧存在,则设置当前帧的IMU的bias初值与上一帧相同
IMU模式且没有新创建地图的情况下对IMU数据进行预积分
拿到两两帧之间待处理的预积分数据,组成一个集合
初始条件判定
上一帧不存在,则不进行预积分,return
拿到的数据集为空,则不积分,return
得到的数据集,去掉太早和太晚的两帧
构成两帧间的imu数据集mvImuFromLastFrame
对两帧之间IMU数据集进行中值积分处理
构建IMU预处理器,并初始化标定数据
清空距离上一帧的预积分运算
for(对组内的每一个IMU数据)
对不同的位置的IMU数据采取不同处理手段
最前和最后两个数据加权处理
其他数据正常处理
计算与上一个IMU的预积分量(两个矩阵,5个雅可比)
mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc,angVel,tstep); pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc,angVel,tstep);
输出距离上一帧的预积分运算和累计距离上一个关键帧的预积分运算
地图更新检测
if(mState)
mState==NOT_INITIALIZED 初始化
单目初始化
第一有效帧设为 Initial Frame 创建 Initializer 对象
第二有效帧设为 Current Frame
ORBmatcher::SearchForInitialization()(Initial Frame 与 Current Frame 之间做特征点匹配)
if (如果匹配特征点对数 < 100)
放弃当前的Initial Frame 和 Current Frame,返回等待新帧进行初始化
if (如果匹配特征点对数 >100)
三角化初始帧和匹配帧
ReconstructWithTwoViews()
Initializer::Initialize()
两个线程同时计算(同时计算 IF 和 CF 之间的 Fundmental Matrix 和 Homography)
Initializer::FindHomography()
Initializer::FindFundamental()
if(RH = SH/(SH+SF))(计算使用两种不同模型的重投影得分,哪个高,就用哪个模型计算出 R 和 t)
> 0.4
ReconstructH()
< 0.4
ReconstructF()
计算地图点坐标:Initialize函数会得到mvIniP3D,mvIniP3D是cv::Point3f类型的一个容器,是个存放3D点的临时变量
初始化的第一帧为世界坐标系
mCurrentFrame.SetPose(Tcw);
Tracking::CreateInitialMapMonocular() (以 Initial Frame 作为地图坐标原点,创建初始的单目地图,地图初始化)
把初始帧和当前帧(mInitialFrame,mCurrentFrame)作为关键帧(pKFin,pKFcuri)插入
创建 两个KeyFrame 对象
把关键帧的描述子转为BoW
KeyFrame::ComputeBoW()
在地图中依次增加这两个关键帧
Map::AddKeyFrame()
用初始化的两帧的匹配点来生成初始地图点
for (每一个有效匹配点) (根据两个 KFs 匹配的特征点生成 MapPoints)
创建 MapPoint 对象
用匹配点的世界坐标构建地图点
在对应帧中的MapPoint中增加地图点和对应索引
pKFini->AddMapPoint(pMP,i); pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
地图点操作

增加该地图点被观测到的帧,以及对应的特征点索引;
选择最佳描述子
更新该地图点的平均观测方向和地图深度
对应地图增加地图点
Map::AddMapPoint()
更新这两个关键帧(pKFin,pKFcuri)的边
遍历当前帧所有的地图点集(未被三角化的为空)
统计其他关键帧看到当前关键帧中点的个数
最后的KFcounter 第一个参数表示某个关键帧,第2个参数表示该关键帧看到了多少当前帧的地图点,也就是共视程度
把共视度>15的关键帧建立相互的连接,找到所有关键帧中看到当前点数最多的关键帧
vPairs记录与其它关键帧共视帧数大于th的关键帧 pair<int,KeyFrame*>将关键帧的权重写在前面,关键帧写在后面方便后面排序
如果没有连接到关键(超过阈值的权重),则对权重最大的关键帧建立连接
排序(大端和下端排序都有)
vPairs里存的都是相互共视程度比较高的关键帧和共视权重,由大到小进行排序
更新当前关键帧和其他关键帧的连接权重
更新生成树链接
把与自己共识度最高的关键帧作为父帧
该父帧增加该帧为孩子节点,
使用一次 Global BA 来对 KFs 位姿和 MapPoints 坐标进行优化
Optimizer::GlobalBundleAdjustmnt()
取场景的中值深度,用于尺度归一化
归一化两帧之间的变换
归一化显示地图3D点的高度
新建预积分
把初始帧和当前帧进行双向链接
建立新的mpImuPreintegrated
new IMU::Preintegrated(pKFcur->mpImuPreintegrated->GetUpdatedBias(),pKFcur->mImuCalib)
局部地图
插入初始双帧
最后关键帧为当前帧
本地关键帧集 双帧
局部地图点为初始地图所有点
Map::SetReferenceMapPoints()
设置 mState = OK
mState==OK||LOST 系统已经经过初始化,常规运行
初步位姿计算
如果为同步建图模式
mState==ok(跟踪正常)
检查并更新建图线程对上一帧MapPoints替换
选择跟踪模型进行跟踪
如果运动模型为空并且IMU未完成初始化 || 重定位之后两帧
用最近参考关键帧(上一帧的参考关键帧)的地图点来对当前普通帧进行跟踪TrackReferenceKeyFrame()
把当前帧的描述子转为BOW向量
构建ORBmacher,vpMapPointMatches存储匹配对,设置当前帧的mvpMapPoints为匹配对,设置当前帧的位姿初始值为上一帧的位置
通过优化3D-2D的重投影误差来获得当前帧位姿,标记特征点中不符合的点
剔除这些外点
返回
IMU直接返回true
其他要求匹配后内点>20
无特殊情况
使用恒速模型跟踪
选择位姿估计方式,估算位姿
IMU完成初始化&&距离重定位较远,IMU不需要重置
根据IMU得到位姿并返回 PredictStateIMU()
其他
利用之前估算的速度和上一帧位姿估算当前的初始位姿
清空当前帧地图点
设置搜索半径
进行投影匹配
匹配<20,扩大2倍搜索半径,重新投影匹配
匹配仍<20
IMU 模式返回true
其他返回 false
>20
3D-2D投影优化当前帧位姿
剔除地图中外点
返回 true
恒速模型失败,使用最近关键帧跟踪
mState==ok
如果为仅跟踪模式
局部地图跟踪
更新局部地图
设置视图中的局部参考点为红色
更新局部地图中的参考帧
得到观测到该点的关键帧和在关键帧中的索引
如果IMU未初始化||完成重定位后2帧
遍历当前帧的所有特征点
每个特征点转为MapPoint
得到观测到该点的关键帧和在关键帧中的索引
如果IMU初始化完成
遍历上一帧的所有特征点
如果该特征点为地图点
得到观测到该点的关键帧和在关键帧中的索引
构建局部关键帧(最大为80)
把观测到当前帧地图点的共视关键帧加入当前帧的局部关键帧,并找到其中观测最多的作为参考关键帧pKFmax
如果不足,则把该帧所有共视关键帧自己的共视帧前10加入局部关键帧中,并加入自己的父关键帧和子关键帧
在IMU模式中,如果此时还不足,则加入临时关键帧
用keyframeCounter 第一个参数表示某个关键帧,第2个参数表示该关键帧看到了多少当前帧(mCurrentFrame)的地图点,也就是共视程度
更新局部地图关键点集
遍历局部地图关键帧,把其地图关键点加入到局部地图点集里
局部地图点映射
遍历标记当前帧中的地图点,不参与后续点映射
添加地图点集里在当前帧视野中的点为待代映射的点
投影匹配,在不同的阶段设置不同的阈值
第二次BA优化
判断跟踪是否成功
跟踪成功
更新恒速模型
mVelocity = mCurrentFrame.GetPose() * LastTwc;
遍历更新当前帧中地图点观测,并统计匹配数目
更新状态参数,mTrackingState,mTrackingMapPoints,mTrackKeyPointsUn
mTrackingState = mpTracker->mState; mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints; mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
返回Tcw
LocalMapping
设置不接受关键帧
如果待处理关键帧列表不为空,并且IMU正常
处理列表中的关键帧,包括计算BoW、更新观测、描述子、共视图,插入到地图等
LocalMapping::ProcessNewKeyFrame()
取出 New KFs 队首元素
KeyFrame::ComputeBoW()
更新 MapPoints 与 KeyFrame 的关联
KeyFrame::UpdateConnections()
Map:::AddKeyFrame()
根据地图点的观测情况剔除质量不好的地图点
创建新的地图点:当前关键帧与相邻关键帧通过三角化产生新的地图点
设置mbAbortBA = false
待处理关键帧列表为空
SearchInNeighbors()
待处理关键帧列表为空 && 闭环检测没有请求停止LocalMapping
当前地图中的关键帧数目>2
处于惯性模式&&当前地图已经完成第一阶段初始化
计算前序临近光心距离和
: 上一个关键帧光心到当前关键帧的光心之间的距离+上上一个关键帧光心到当前上一个关键帧光心之间的距离
如果距离>1.5,则累加计算IMu累计时间
当前关键帧所在的地图尚未完成IMU BA2(IMU第三阶段初始化):
!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()
如果经过10msIMU累计,当前计算的光心距离仍然小于2,则重置地图,设置badIMU位
判断跟踪点数目是否足够:单目下追踪75个点,其他追踪1100个点
局部地图+IMU一起优化:Optimizer::LocalInertialBA
其他
局部地图BA,不包含IMU信息
惯性模式&&未完成第一阶段初始化 if(!mpCurrentKeyFrame->GetMap()->isImuInitialized())
第一次初始化

如果为mbResetRequested
退出
设置最小间隔时间和最小累计帧数
如果地图中的关键帧<最小累计帧数
退出
依时序构建地图关键帧的列表和容器
如果关键帧时戳差<最小时差,则退出
置bInitializing为true
处理有可能新到的关键帧
当前IMU没有进行任何初始化
根据当前存储的所有帧进行IMU初始化
Eigen::Matrix3f Rwg; Eigen::Vector3f dirG; dirG.setZero();
判断当前帧带有IMU预积分信息
判断当前帧不是最开始的帧
对于每一组顺序临近关键帧
计算速度的预积分
计算速度的观测量
如果带有IMU预积分信息的帧少于6,初始化失败
对预积分归一化
求预积分在自身世界坐标系和相机的世界坐标系下的夹角
获得重力坐标系到世界坐标系的旋转矩阵
恢复尺度信息和重力方向
更新当前帧和上一帧位姿
检测并剔除当前帧相邻的关键帧中冗余的关键帧:该关键帧的90%的地图点可以被其它关键帧观测到
惯性模式下如果关键帧累计时间<50
如果当前帧所在地图完成IMU初始化,并且跟踪正常
如果当前关键帧所在的地图还未完成VIBA 1
如果帧累计时间>5
当前帧地图进行VIBA 1,IMU进行第二阶段初始化
第二次初化化

如果为mbResetRequested
退出
设置最小间隔时间和最小累计帧数
如果地图中的关键帧<最小累计帧数
退出
依时序构建地图关键帧的列表和容器
如果关键帧时戳差<最小时差,则退出
置bInitializing为true
处理有可能新到的关键帧
当前IMU没有进行任何初始化
根据当前存储的所有帧进行IMU初始化
Eigen::Matrix3f Rwg; Eigen::Vector3f dirG; dirG.setZero();
判断当前帧带有IMU预积分信息
判断当前帧不是最开始的帧
对于每一组顺序临近关键帧
计算速度的预积分
计算速度的观测量
如果带有IMU预积分信息的帧少于6,初始化失败
对预积分归一化
求预积分在自身世界坐标系和相机的世界坐标系下的夹角
获得重力坐标系到世界坐标系的旋转矩阵
恢复尺度信息和重力方向
更新当前帧和上一帧位姿
如果当前关键帧所在的地图还未完成VIBA 2
如果帧累计时间>15
当前帧地图进行VIBA 2,IMU进行第三阶段初始化
如果关键帧数目小于200,在一定的时间之后就进行尺度和重力方向的优化
将当前帧加入到闭环检测队列中
IMU初始化标志
第一阶段:mbImuInitialized
第二阶段:mbImu_BA1
第三阶段:mbImu_BA2
第四阶段:在单目相机模式下增加了单独的重力方向优化和尺度