ORB-SLAM2梳理——track线程（一）

本科毕设所研究的是基于直接法的视觉里程计技术，采用的开源方案是DSO（Direct Sparse Odometry）。在引入光度校准和光度仿射变换之后，并且联合优化了所涉及的所有参数，DSO的效果很棒，被誉为目前效果最好的直接法的方案，但是它只是一个odometry，并不是一个完整视觉slam系统。因此，为了更好的研究视觉SLAM，对ORB-SLAM2的论文以及代码进行了研究，在这里写下自己的一些拙见，欢迎各位大神指出文中的不足和错误之处。

以ORB-SLAM2中传感器为单目相机运行EuRoc数据集为例分析流程。
首先分析运行参数，一共需要五个参数，并且需要按照特定的顺序排列，

argv[0]	可执行文件
argv[1]	ORB词典文件 strVocFile
argv[2]	参数设置文件 strSettingsFile
argv[3]	图像文件 vstrImageFilenames
argv[4]	时间戳 vTimestamps

加载图片和曝光时间：LoadImages(string(argv[3]), string(argv[4]), vstrImageFilenames, vTimestamps);
系统初始化：ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);   //读取配置文件
mpVocabulary = new ORBVocabulary();bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);  //构建orb字典对象，读取orb字典
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);  //创建关键帧数据库
mpMap = new Map();  //创建地图
mpFrameDrawer = new FrameDrawer(mpMap);  mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);  // 可视化部分
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
//track线程，在这个线程会读取配置文件进行相机内参矩阵的构建，畸变向量的构建，还会根据配置文件设置一些参数。
//最重要的是会构建两个特征提取的对象：mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);  mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);  //对于传感器为单目相机所特有，用于初始化部分的特征点提取，提取数量是设置的两倍。
另外此部分还会初始化另外两个线程，目前还未仔细去看，在分析完track线程之后再讨论

利用for循环遍历所有图像帧，图像帧处理入口：SLAM.TrackMonocular(im,tframe);
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);
如果是第一帧：（或者未初始化成功）
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
参数：灰度图、时间戳、初始ORB特征提取对象、ORB词典对象、相机内参矩阵、畸变向量、剩下两个参数不清楚（看yaml文件里也没有，可能对于单目相机不重要）

ExtractORB(0,imGray);//  提取ORB特征(*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);  //orb特征提取采用的是重载的运算符()。
UndistortKeyPoints();  //对特征点去畸变cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);  //利用opencv提供的函数去畸变
ComputeImageBounds(imGray)  //计算去畸变之后图像的边界，只对第一帧执行。
AssignFeaturesToGrid();  //将关键点划分到格子中

如果不是第一帧：（或者初始化成功）
mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
将ORB提取对象换成了mpORBextractorLeft，其他基本不变。
在构建图像帧对象Frame()之后，开启跟踪。构建的对象为mCurrentFrame，此时该图像帧已经提取了特征点并计算了相应的描述子，具体的实现过程后续讨论。
开始跟踪：Track()；如果还未进行初始化，首先进行初始化操作，初始化操作需要连续的两个帧，并且两个帧的特征点匹配对数要大于100对，之后在进行三角化获得初始深度，构建初始地图。如果不满足条件则初始化失败，等下后续满足条件的连续两帧。

MonocularInitialization(); //单目初始化对于第一帧：if(mCurrentFrame.mvKeys.size()>100)  //特征点个数要大于100个{mInitialFrame = Frame(mCurrentFrame);mLastFrame = Frame(mCurrentFrame);mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());  //调整vector的大小for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;    //存储了mInitialFrame中哪些点将进行接下来的匹配if(mpInitializer)delete mpInitializer;mpInitializer =  new Initializer(mCurrentFrame,1.0,200);//这里实例化Initialize类的一个对象指针，初始化的值为选取的当前帧和测量误差1.0，RANSAC迭代次数200fill(mvIniMatches.begin(),mvIniMatches.end(),-1);return;}对于第二帧：if((int)mCurrentFrame.mvKeys.size()<=100)    特征点个数要大于100个，如果小于100，初始化失败，程序返回图像帧处理入口。{delete mpInitializer;mpInitializer = static_cast<Initializer*>(NULL);fill(mvIniMatches.begin(),mvIniMatches.end(),-1);return;}ORBmatcher matcher(0.9,true);//对两帧的特征点进行匹配，参数：第一帧(初始帧)，第二帧(当前帧)，前一帧的特征点，两帧匹配特征点的索引，窗口大小(此参数不太了解)int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);if(nmatches<100)    //如果匹配对数小于100，不能初始化成功，即初始化失败，程序返回图像帧入口。{delete mpInitializer;mpInitializer = static_cast<Initializer*>(NULL);return;}mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)  //当初始化的条件满足之后，执行初始化。//初始化成功之后，将会删除初始化操作中三角化未成功的匹配点对，然后建立初始地图。 CreateInitialMapMonocular();
//至此，单目初始化的流程已全部走完，具体实现后续讨论。

在初始化成功之后，后续到来的帧，利用下面的代码构建帧Frame对象（前面已经提到过，这里再讲一下）。

mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

构建了帧Frame的对象之后，执行Track()。
接下来进行跟踪ORB-SLAM中关于跟踪状态有两种选择（由mbOnlyTracking判断）
1 只进行跟踪不建图，此时mbOnlyTracking为true
2 同时跟踪和建图，此时mbOnlyTracking为false。
初始化之后ORB-SLAM有三种跟踪模型可供选择
(a) TrackWithMotionModel(); 运动模型：根据运动模型估计当前帧位姿，根据匀速运动模型对上一帧的地图点进行跟踪，优化位姿。
(b) TrackReferenceKeyFrame(); 关键帧模型：BoW搜索当前帧与参考帧的匹配点，将上一帧的位姿作为当前帧的初始值，通过优化3D-2D的重投影误差来获得位姿。
(c）Relocalization()；重定位模型：计算当前帧的BoW，检测满足重定位条件的候选帧，通过BoW搜索当前帧与候选帧的匹配点大于15个点就进行PnP位姿估计并优化。

if(!mbOnlyTracking) //进行跟踪和建图 {// Local Mapping is activated. This is the normal behaviour, unless// you explicitly activate the "only tracking" mode.if(mState==OK)  {// Local Mapping might have changed some MapPoints tracked in last frame  CheckReplacedInLastFrame();  // 检查并更新上一帧被替换的MapPoints，更新Fuse函数和SearchAndFuse函数替换的MapPointsif(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)  //当运动模型是空的，或者刚刚使用重定位{bOK = TrackReferenceKeyFrame();   //使用参考关键帧模型跟踪}else{bOK = TrackWithMotionModel();  //使用运动模型跟踪if(!bOK)     //如果使用运动模型跟踪失败bOK = TrackReferenceKeyFrame();   //使用参考关键帧模型}}else   //{bOK = Relocalization();  //重定位}}
else    //只跟踪，不建图{if(mState==LOST){bOK = Relocalization();}else{if(!mbVO){if(!mVelocity.empty()){bOK = TrackWithMotionModel();}else{bOK = TrackReferenceKeyFrame();}}else   这一部分的内容在mbVO=true的时候执行，具体不是太清楚，后续在研究。{// In last frame we tracked mainly "visual odometry" points.// We compute two camera poses, one from motion model and one doing relocalization.// If relocalization is sucessfull we choose that solution, otherwise we retain// the "visual odometry" solution.bool bOKMM = false;bool bOKReloc = false;vector<MapPoint*> vpMPsMM;vector<bool> vbOutMM;cv::Mat TcwMM;if(!mVelocity.empty()){bOKMM = TrackWithMotionModel();vpMPsMM = mCurrentFrame.mvpMapPoints;vbOutMM = mCurrentFrame.mvbOutlier;TcwMM = mCurrentFrame.mTcw.clone();}bOKReloc = Relocalization();if(bOKMM && !bOKReloc){mCurrentFrame.SetPose(TcwMM);mCurrentFrame.mvpMapPoints = vpMPsMM;mCurrentFrame.mvbOutlier = vbOutMM;if(mbVO){for(int i =0; i<mCurrentFrame.N; i++){if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]){mCurrentFrame.mvpMapPoints[i]->IncreaseFound();}}}}else if(bOKReloc){mbVO = false;}bOK = bOKReloc || bOKMM;}  这一部分的内容在mbVO=true的时候执行，具体不是太清楚，后续在研究}}

在相应的跟踪状态下，采用相应的跟踪模型得到姿态初始估计之后，跟踪LocalMap，得到更多的匹配，并优化当前位姿。LocalMap：当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系，在步骤2.1中主要是两两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints，然后将局部MapPoints和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化

if(!mbOnlyTracking)
{if(bOK)bOK = TrackLocalMap();
}
else
{// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes// the camera we will use the local map again.if(bOK && !mbVO)bOK = TrackLocalMap();
}

在TrackLocalMap()之后；更新恒速运动模型TrackWithMotionModel中的mVelocity。

if(!mLastFrame.mTcw.empty(){cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));mVelocity = mCurrentFrame.mTcw*LastTwc;}

判断是否要新加关键帧。

if(NeedNewKeyFrame())CreateNewKeyFrame();

至此，orb-slam2的track线程基本走完，本文着重讨论的track线程的流程梳理。每个流程的具体实现的函数及代码在后续博客中讨论，其中包括：

///  ORBextracor的构造函数
mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
///  提取ORB特征的重载运算符()
(*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);
///   特征点匹配函数
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
// 初始化函数
mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)
//建立初始单目地图
CreateInitialMapMonocular();
//   三种跟踪模型
TrackReferenceKeyFrame()
TrackWithMotionModel()Relocalization()
/// 跟踪局部地图
TrackLocalMap()
///  判断是否要加关键帧以及如何创建关键帧
if(NeedNewKeyFrame())CreateNewKeyFrame();

ORB-SLAM2梳理——track线程（一）相关推荐

ORB_SLAM3 Track线程详解
概述 ORB-SLAM3 是第一个同时具备纯视觉(visual)数据处理.视觉+惯性(visual-inertial)数据处理.和构建多地图(multi-map)功能,支持单目.双目以及 RGB-D ...
ORB SLAM2源码解读(三)：Frame类
文章目录前言构造函数双目相机 RGBD相机单目相机 ExtractORB:提取特征点 ComputeBoW:计算词袋数据 SetPose:设置相机外参 isInFrustum:判断一个MapP ...
orbslam2可视化_[Ubuntu] ORB SLAM2 编译调试
ORB SLAM2 是 2015年比较受到关注的一篇文章,它的主要思想是借助 ORB 描述子改进了 Sparse SLAM 的性能,使得其在稳定性和速度上都达到了比较好的程度.从创新性上来讲,它的主要 ...
Ubuntu下使用单目相机运行ORB SLAM2
环境:Ubuntu16.04+ROS Kinetic+USB单目摄像头虽然ORB SLAM2的官方说明中表示没有ROS也可以编译运行,但要实时的跑ORB SLAM2还是需要ROS平台的,所以之前没有 ...
新手入门新电脑安装配置orb slam2 一步到位不踩坑（ubutun18.04）
目录一.安装基础工具二.安装Pangolin,作为可视化和用户界面三.安装OpenCV,用于处理图像和特征 (1)下载OpenCV 3.4.1 (2)安装依赖项 (3)编译安装 (4)配置环境, ...
Ubuntu 16.04~ORB SLAM2～Kinect v1
额 Ubuntu16.04 ORB-SLAM2实现(kinect V1/ROS) ----------------------------------------------------------- ...
Ubuntu14.04 使用本地摄像头跑ORB SLAM2
嗯这个方法我暂时弄不出来,用了另外一个方法:SLAM14讲第一次课使用摄像头或视频运行 ORB-SLAM2 前面的准备: Ubuntu14.04安装 ROS 安装步骤和问题总结 Ubuntu14 ...
视觉SLAM融合GPS尝试
一.前言最近在做无人机建图的相关工作,基本的方案是ORB SLAM2+Map2DFusion. 在调试好代码后,我利用大疆精灵4在附近的一个公园进行算法测试,得到的效果图如下: 但在一些细节上会有明 ...
orb特征稠密特征_一种基于ORB-SLAM2的双目三维稠密建图方法技术
本发明专利技术公开了一种基于ORB‑SLAM2的双目稠密建图方法,涉及机器人同步定位与地图创建领域,该方法主要由跟踪线程.局部地图线程.闭环检测线程和稠密建图线程组成.其中稠密建图线程包含以下步骤:1 ...

ORB-SLAM2梳理——track线程（一）

ORB-SLAM2梳理——track线程（一）相关推荐

最新文章

热门文章