之前学习VINS和LIO-SAM的时候都是代码流,不是很重视看论文,现在有空学ORB-SLAM3了,这一次,先看一下论文。考虑到边上班边学,更新的会比较慢。看完论文之后,对这个系统最大的主观感受就是它的多图系统Atlas。机器人运行过程中是可能会跟丢的,一旦跟丢了就新建一个map,如果走到之前的位置,一旦发现和之前的map是同一个地方,就可以合并map。它的初始化和VINS很像,地图操作,回环检测,重定位很鲁棒。
github: https://github.com/UZ-SLAMLab/ORB_SLAM3
论文: https://arxiv.org/pdf/2007.11898.pdf
相关代码解析如下(未完待续):
【SLAM】ORB-SLAM3解析——关键帧Frame()的构建(2)
【SLAM】ORB-SLAM3解析——视觉里程计Track()(3)
【SLAM】ORB-SLAM3解析——Mapping(4)
Abstract
ORB-SLAM3有以下特点:基于ORB-SLAM2和ORB-SLAM-VI开发的,支持V-SLAM(mono, stereo),VI-SLAM,RGBD-SLAM模式,相机可以是针孔或鱼眼。
它的第一大特点是基于特征点的VI-SLAM,依赖极大后验估计MAP,哪怕和IMU的初始化都用MAP,一说这个我就想到了最小二乘和BA。
它的第二大特点是具备multiple map system,所以说它的定位精度,回环检测,重定位能力很顶。
I.Introduction & II.Related Work
现在的V-SLAM和VO都依赖于极大后验估计MAP,具体的来说就是Bundle Adjustment最小化重投影误差。V-SLAM的目的既包括状态估计也包括建图,而VO更关注于状态估计。
V-SLAM的优势体现在它能利用上以下三个信息:
①短期数据匹配:短时间内一帧和一帧之间的联系来恢复运动。这是V-SLAM和VO的一个共同点,不过长时间会造成漂移。
②中期数据匹配:把当前相机状态和全局地图中没有漂移的那部分进行匹配。这比带回环检测的VO效果都好。
③长期数据匹配:回环检测。
说白了,就是当前的状态估计分别可以用 当前帧 与 次新帧,比较准的全局地图(同时也可能比较新) 和 比较久远的全局地图 分别匹配进行BA,这样定位精度很高没有drift,作者说这是ORB-SLAM3的优点。
它的重定位和一般用词袋模型的VIO/VO不一样,词袋模型是先根据词袋匹配,再判断位置一致性,这样的缺点是慢(VINS好像就是这么干的)。
ORB-SLAM3先根据位置找可能的回环(可能是对定位很自信吧),然后再用3个共视关键帧确定回环,这么做会更精确,但是会有点费算力。
Atlas,也就是multiple map system,结合了重定位,地图识别,回环检测等技术,用这些方式可以把几块地图(可以是几次不同的运行)拼在一起。所以一旦跟丢了,系统就建一个新图,如果走着走着和之前匹配上了就可以再连起来。
ORB-SLAM3用ORB特征进行短期,中期的数据进行匹配;通过创建的共视图covisibility graph可以减少跟踪和建图的复杂度,回环检测和重定位用了词袋实现长期的数据匹配;作者说ORBSLAM3是唯一集齐以上3个手段的系统,是它定位准确的关键所在。
ORB-SLAM3的初始化是基于ORB-SLAM-VI并拓展成stereo的,基于极大后验估计,2s做到误差5%,15s做到误差1%。
III.System Overview
ORB-SLAM3包括以下几个部分:
Atlas:也就是多地图系统,由一系列离散的地图组成。其中有一个active map,tracking线程基于这个地图进行定位,local map线程会把新的关键帧加到这个地图里;除此以外还有很多non-active map;还有一个DBoW2关键帧数据集用于重定位,回环检测和地图合并。
Tracking:处理传感器数据;通过最小化重投影误差实时计算最新位姿;选择关键帧;VI模式下,计算body速度和IMU误差;如果跟丢了,在Atlas多图系统中进行重定位,成功了就切换对应地图,失败了就新建一个地图。
Local Mapping:把新的关键帧和地图点加到active map里,删掉多余点和多余关键帧;在最新的滑窗内进行BA优化地图;初始化时估计IMU参数。
Loop和Map Merging:以关键帧的频率搜索active map和Atlas里其它map的共同区域,如果这个共同区域属于active map,就搞一次回环矫正,如果属于别的map,这两个map合并,然后变成active map;回环矫正后,会新建一个线程进行全局BA优化位姿。
上图是ORB-SLAM3的总体流程。Tracking是前端,包括IMU预积分,关键帧的构建,然后是Local Mapping,这两步就构成了最基础的里程计。图片中间是它最具特色的Atlas多图系统,包括DBoW2词袋数据库,当前正在使用的Acitive Map,和历史保存的多个Inactive Map。一旦在Loop中发现了Place recognition,就会把Active Map和Loop Map进行merging,再进行Full BA。
IV.相机模型
整个系统是假设为针孔模型的,所以为了适用其它相机模型(鱼眼),把相机抽象出来。对于鱼眼相机而言(FOV>180°),去畸变会出现问题(外围物体放大,中间物体损失分辨率)。如果要像针孔相机那样去畸变,就要把照片外围裁掉,会损失信息。
对于双目而言,这个问题的影响更大,为了解决这个问题,他们把双目是为两个单目,而这两个单目之间有着固定的SE3和共视区。开始时通过共视区特征的三角化获得真实scale,之后就按照单目的玩法来。
V.VI-SLAM
A. 公式
相较于ORB-SLAM-VI,当前系统的初始化的速度更快了。在V-SLAM里,只用估计相机位姿,但是在VI-SLAM里,还需要估计body位姿,世界坐标系下的速度,IMU误差,同样需要i到i+1帧之间进行IMU预积分。公式(2)表示的是由IMU获得的状态关系所计算的残差。
公式(3)表示由相机模型获得的重投影误差,如果给定一系列的关键帧,状态量和路标点,那么优化问题就是最小化IMU误差和重投影误差,如公式(4),是不是很眼熟。
B. IMU初始化
目标是获得body速度,重力方向,IMU bias。光看论文感觉这初始化的办法和VINS-Mono的很像啊。
初始化有3个关键点:
->纯单目初始化能提供非常精确的map,仅scale不知道;
->如果把scale当成一个被优化的变量,速度会更快;
->不能忽视IMU的误差。
所以,他们把IMU初始化作为一个MAP估计问题,并分为3步:
①仅有视觉的MAP估计:2s进行mono-slam初始化,以4Hz插入关键帧,获得10个相机位姿和几百个点,然后来一个纯视觉BA。
②仅有IMU的MAP估计:优化目标是重力向量,IMU bias,尺度,速度。公式(8)是优化目标。
③VI MAP估计:这部分论文说的不是很具体,到时候看看代码是咋实现的把。
对于初始化时运动量过小导致初始化失败的情况,他们就只优化尺度和重力,至于IMU bias就在优化部分处理了。
C. Tracking & Mapping
Tracking: 最后2帧会被优化,路标点会被固定。
Mapping: 维护了一个滑窗,pose会被固定,路标点会被优化。
D. Track Loss
暂时跟丢了:根据IMU估计位姿,把已有地图点根据这个位姿投影到照片上进行匹配,然后优化位姿,连这都不行那就是跟丢了。
彻底跟丢了:重开一张新地图。
VI.地图合并和回环检测部分
短期和中期的数据匹配是通过重投影实现的,长期数据匹配依赖词袋。在传统的词袋模型回环检测中,先根据词袋确定潜在回环,为了提高准确率然后再根据位置关系筛选,从而做到100%准确率和30-40%的召回率。
但是这么玩的结果就是认出的回环帧会延后3帧,而且总在相同的位置找到回环(不懂)。
ORB-SLAM3的Atlas这么搞:
每来一个关键帧,就在Atlas里找关联,如果关联的关键帧在active map里(见D.),就来一次回环矫正,否则就把active map和被关联的关键帧所在的map进行合并(见B.和C.)。
一旦回环关系找到了,就分别把回环两帧和各自(共视图里的)相邻帧捞出来,他们作为一个local window,在其中进行稠密的mid-term数据匹配。
A. 回环检测
ORBSLAM3这部分要比VINS复杂的多。
(1)利用DBoW2词袋找到与重定位帧Ka最像的3帧Km做为备胎回环帧,它们不能是重定位帧Ka的共视帧;
(2)对于每一个Km,都能借助DBoW2找到它与Ka之间共视路标点的对应关系,包括2D和3D坐标;
(3)根据匹配点+RANSAC计算Km帧到Ka帧的位姿变换Tam;我们知道RANSAC每次迭代会随机取少量的数据去拟合一个模型,用剩下的点计算和这个模型的距离确定是它是内点还是外点,最后内点数最多的模型为结果。在这里,重投影误差小于阙值的点会作为内点;
(4)Tam求出来后,各自的特征点相互之间重投影找到更多的匹配关系,然后玩一次非线性优化来优化Tam;
(5)为了避免假阳性,会在active map里再在Ka帧的共视帧找2个小伙伴,与回环帧验证回环关系;
(6)对于带IMU的模式,会增加pitch和roll约束限定匹配关系;
B. Visual模式下的Map合并
重定位帧Ka的map是Ma(active map),回环帧Km的map是Mm。B.和C.针对的是重定位帧回环帧在不同的map下这种情况。
(1)将Ma和Mm合并到同一个window下。上一步把Tma求出来了,那么就把Ka帧和它的共视帧,特征点都用Tma变换到Mm的坐标系下;
(2)合并之后就会有重复的路标点,那么就把Ka帧对应的重复点删掉,之后利用mid-term的数据匹配建立共视图;
(3)在这个window里进行BA,Mm中那些不在window里的关键帧的pose会被固定住,其余所有window中的路标点和关键帧pose都会被优化;
(4)全局位姿图优化。window里的pose会被固定,其余pose会被优化。
C. VI模式下的Map合并
和Visual模式下的Map合并基本一样,就是(1)(3)会有补充操作;
(1)如果Ma成熟了,那么Tma是SE(3),否则是Sim(3);
(3)优化项增加了速度和bias。Mm中,window之前紧挨着的5帧会被固定住不优化,所有的路标点,Km,Ka帧和他们的共视帧都会根据重投影模型被优化。
D. Loop Closing
本部分针对的是重定位帧回环帧都在active map下这种情况。对于匹配上的两帧同样会创建一个window,它们和共视路标点也会构造一个共视图,然后对map中其它帧进行一次位姿图优化。
最后根据mid-term和long-term的数据匹配关系进行一次全局BA。对于VI模式,全局BA仅在关键帧数量不多时进行,以控制计算量。
1. 系统入口
入口函数都在\Examples\目录下,这里以Stereo-Inertial\stereo_inertial_euroc.cc为例逐渐进入到系统内部,其它的入口函数都是类似的。我们看他的main(),开始时会创建一系列输入数据的容器,保存IMU的数据和image的路径。然后就会创建一个System对象:
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO, false);
之后,把一对image,时间戳<image的IMU数据(a,w,time)组成一帧数据,进行当前轮次的track:
// Pass the images to the SLAM system
SLAM.TrackStereo(imLeft,imRight,tframe,vImuMeas);
1.1 System对象的构建
在路径\src\System.cc。构造函数中,会把整个SLAM系统涉及到的主要功能类(Tracking, Mapping, Loop Closing, Viewer)和数据结构类(Vocabulary, KeyFrame, Atlas)作为成员变量进行实例化。同时,会把相互依赖的功能类的指针传入到对应成员模块的构造函数中。
//Load ORB Vocabulary
mpVocabulary = new ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
//Create KeyFrame Database
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
//Create the Atlas
mpAtlas = new Atlas(0);
//Create Drawers. These are used by the Viewer
mpFrameDrawer = new FrameDrawer(mpAtlas);
mpMapDrawer = new MapDrawer(mpAtlas, strSettingsFile, settings_);
//Initialize the Tracking thread
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpAtlas, mpKeyFrameDatabase, strSettingsFile, mSensor, settings_, strSequence);
//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR, mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO || mSensor==IMU_RGBD, strSequence);
mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper);
//Initialize the Loop Closing thread and launch
mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR, activeLC); // mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);
//Set pointers between threads
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);
//Initialize the Viewer thread and launch
mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile,settings_);
mptViewer = new thread(&Viewer::Run, mpViewer);
mpTracker->SetViewer(mpViewer);
以上几个功能类中,需要注意的是LocalMapping,LoopClosing和Viewer是另外开了3个线程做各自的事情。
1.2 SLAM算法入口函数TrackStereo()
在路径\src\System.cc。根据输入image关于stereo,mono,rgbd的不同,分别执行:
// Proccess the given stereo frame. Images must be synchronized and rectified.
// Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Returns the camera pose (empty if tracking fails).
Sophus::SE3f TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
// Process the given rgbd frame. Depthmap must be registered to the RGB frame.
// Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Input depthmap: Float (CV_32F).
// Returns the camera pose (empty if tracking fails).
Sophus::SE3f TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
// Proccess the given monocular frame and optionally imu data
// Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
// Returns the camera pose (empty if tracking fails).
Sophus::SE3f TrackMonocular(const cv::Mat &im, const double ×tamp, const vector<IMU::Point>& vImuMeas = vector<IMU::Point>(), string filename="");
这里我们以Stereo模式为例。在这个函数里面,首先是对image的预处理,然后确定SLAM运行的模式是纯定位还是定位+建图模式,是否reset系统等,不过,最核心的内容是这4行:
if (mSensor == System::IMU_STEREO)
for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
mpTracker->GrabImuData(vImuMeas[i_imu]);
Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed,imRightToFeed,timestamp,filename);
GrabImuData()的作用是把IMU数据放到mlQueueImuData中。GrabImageStereo()中,最核心的也只有2行:
mCurrentFrame = Frame();
Track();
分别是构建最新帧和进行track。