这里写自定义目录标题
引入
阅读完ORB-SLAM2代码，在此做一个总结。
主要参考了别人对代码的注释：https://gitee.com/paopaoslam/ORB-SLAM2/tree/master
同时，因为笔者主要关注单目图像输入，所以参考论文：《ORB-SLAM: a Versatile and Accurate Monocular SLAM System》
三线程的整体结构
系统包含三个并行的线程：tracking/跟踪，local mapping/局部建图和 loop closing/闭环。
mono_tum.cc 中初始化SLAM系统并运行主循环
int main(){ //mono_tum.cc中
//创建系统SLAM 包括1,创建跟踪线程；2,创建局部建图线程，并启动；3,创建闭环线程并启动
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
for(){//主循环
//读取当前图像
im = cv::imread(string(argv[3])+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
//将当前图像送入跟踪线程
SLAM.TrackMonocular(im,tframe);
}
}
在初始化系统的函数System::System()中，完成了对三个线程的初始化并启动（跟踪线程不需要启动，其实际运行在主线程中）
//System.cc文件中
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,const bool bUseViewer):mSensor(sensor){
//创建ORB字典，并从文件中装载
mpVocabulary = new ORBVocabulary();
mpVocabulary->loadFromTextFile(strVocFile);
//创建关键帧数据库
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
//创建地图
mpMap = new Map();
//创建跟踪线程
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
//创建局部建图线程，并启动
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
//创建闭环线程，并启动
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
//赋值三个线程的相关指针
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);
}
跟踪线程
代码概况
1，跟踪线程，首先在main()函数中的ORB_SLAM2::System SLAM()中完成创建，并在主循环下运行。
int main(){ //mono_tum.cc中
//创建系统SLAM 中引用Tracking()创建跟踪线程
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
for(){//主循环
//读取当前图像
im = cv::imread(string(argv[3])+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
//将当前图像送入跟踪线程
SLAM.TrackMonocular(im,tframe);
}
}
2，然后再次经转两个函数，在Tracking::GrabImageMonocular()中创建当前帧 Tracking.mCurrentFrame，最终进入跟踪 Track()。
//System.cc
cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp){
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);
}
//Tracking.cc
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp){
//创建当前帧
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
//进入跟踪
Track();
}
3，Tracking::Track()是跟踪线程的核心，我将代码分为多块，并逐块注释、拆解。
void Tracking::Track()
{
//跟踪初始化
//frame-to-frame跟踪
//局部地图跟踪
//关键帧选择
}
线程创建
跟踪线程的创建仅是对变量赋予初值
Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor){
// 1首先初始化一下变量
// mState(NO_IMAGES_YET), //一个重要的变量，指示跟踪线程是正常(OK)还是丢失(LOST),初始化为NO_IMAGES_YET,为进行跟踪初始化做准备。
// mSensor(sensor), //传感器对的类型，在此是单目
// mbOnlyTracking(false), //这是一个需要用户介入的设置，如果为1,系统将不会建立局部地图，默认为false
// mbVO(false),
// mpORBVocabulary(pVoc), //BoW字典
// mpKeyFrameDB(pKFDB), //跟踪线程参考帧,一般为上一个关键帧
// mpInitializer(static_cast<Initializer*>(NULL)), //初始化器，跟踪初始化时使用
// mpSystem(pSys), //指向我们建立SLAM系统
// mpViewer(NULL), //用于与用户交互,在此不考虑
// mpFrameDrawer(pFrameDrawer),//用于与用户交互,在此不考虑
// mpMapDrawer(pMapDrawer),//用于与用户交互,在此不考虑
// mpMap(pMap), //指向地图
// mnLastRelocFrameId(0)
// 2从设置文件中载入 跟踪线程 的一些参数：
// cv::Mat mK; //相机内参
// cv::Mat mDistCoef; //相机畸变参数
// float mbf; //相机参数 camera.bf
// int mMinFrames; //两个值用于判断当前帧是否作为关键帧的阈值(距离上一次插入关键帧过去了多少帧)
// int mMaxFrames; //同上
// bool mbRGB; //无关紧要的 传感器判断标志
// 3从设置文件中读取 特征提取器的参数，并初始化特征提取器
// mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
// mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
}
创建帧
由Tracking::GrabImageMonocular()引用。
//Tracking.cc
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp){
//创建当前帧
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
//进入跟踪
Track();
}
主要进行了相关参数的初始化，搜索特征点 Frame.mvKeys/ Frame.mvKeysUn，并计算特征描述符 Frame.mDescriptors，再将特征点分配到网格Frame.Grid 中。
//Tracking.cc
Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
:mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
{
//1,设置相关参数
mnId=nNextId++; //Frame id
// 设置图像金字塔的相关参数
mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
//2,提取ORB特征,并将 特征点/描述符 存储在 Frame.mvKeys/Frame.mDescriptors 中
ExtractORB(0,imGray); //获取特征点 存储在 mvKeys,mDescriptors 中
//Frame.N为特征点数量
N = mvKeys.size();
if(mvKeys.empty())
return;
//3,调用OpenCV的矫正函数矫正orb提取的特征点,结果存储在 Frame.mvKeysUn
UndistortKeyPoints();
//单目没有使用
mvuRight = vector<float>(N,-1);
mvDepth = vector<float>(N,-1);
//存储与特征点关联的世界点
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
//用于判断利群点的标志
mvbOutlier = vector<bool>(N,false);
// 只在第一帧时运行一次,用于记录相机参数
if(mbInitialComputations)
{
ComputeImageBounds(imGray);
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
fx = K.at<float>(0,0);
fy = K.at<float>(1,1);
cx = K.at<float>(0,2);
cy = K.at<float>(1,2);
invfx = 1.0f/fx;
invfy = 1.0f/fy;
mbInitialComputations=false;
}
//双目baseline
mb = mbf/fx;
//4,将特征点分配到一个64*48的网格当中 Frame.Grid
//每个格cell里面存储的是 特征点Frame.mvKeysUn 索引集合
AssignFeaturesToGrid();
}
单目初始化 Tracking::MonocularInitialization()
由图像建立帧后，将会进入 Tracking::Track() 函数进行当前帧的跟踪。
void Tracking::Track()
{
//1,跟踪初始化：需要连续两帧有足够的匹配点,初始化跟踪相关参数
if(mState==NO_IMAGES_YET)//创建跟踪线程时，mState=NO_IMAGES_YET。即，第一次运行时进入,需要进行跟踪初始化
{mState = NOT_INITIALIZED;}
mLastProcessedState=mState;
if(mState==NOT_INITIALIZED)//第一次运行时候/或没有初始化成功时
{
//进行单目初始化,如果成功,mState=OK
MonocularInitialization();
if(mState!=OK) //如果没有成功 mState保持为NOT_INITIALIZED,则尝试初始化
return;
}
...//初始化后
即，第一次跟踪时（mState=NO_IMAGES_YET），或初始化未成功时（mState=NOT_INITIALIZED）会不断尝试进行单目初始化，其目的是找到连续两帧满足条件的图像作为系统的初始。
//Tracking.cc中
//第一次跟踪时,进行单目初始化,其目的是找到连续两帧满足条件的图像作为系统的初始
void Tracking::MonocularInitialization()
{
if(!mpInitializer) //初始化器,创建跟踪线程或初始化失败时设为NULL,即跟踪尚未初始化成功
{
//1,测试当前帧(第一帧),如果满足条件(特征点数量大于100),建立初始化器
if(mCurrentFrame.mvKeys.size()>100) //单目初始帧提取的特征点数必须大于100，否则放弃该帧图像
{
mInitialFrame = Frame(mCurrentFrame); //赋予 Tracking.mInitialFrame 当前帧,在与下一帧进行匹配时使用.
mLastFrame = Frame(mCurrentFrame); //将当前帧保存在 Tracking.mLastFrame 中
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size()); //用于存储当前帧特征点集
for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
mpInitializer = new Initializer(mCurrentFrame,1.0,200); //由当前帧 构造 初始化器 Tracking.mpInitializer
fill(mvIniMatches.begin(),mvIniMatches.end(),-1); // mvIniMatches
return;
}
//else： 如果特征点数量不足， 不改变 mState 状态，系统将返回主循环获得新帧，并继续尝试初始化
}
else
{
//2,在前一帧成功建立初始化器时,测试当前帧(第二帧)是否满足条件,如果满足则匹配当前帧和前一帧.
if((int)mCurrentFrame.mvKeys.size()<=100)
{ //失败——第二张不满足条件，删去“初始化器”，并设置为NULL，重新尝试初始化
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
//匹配当前帧和前一帧 匹配关系存储在 mvIniMatches
ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
//失败——匹配太少
if(nmatches<100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
return;
}
cv::Mat Rcw;
cv::Mat tcw;
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
//3,当连续两帧的匹配点数量满足条件,通过H模型或F模型进行单目初始化,得到两帧间相对运动Rcw,tcw
if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
{
//4,删除那些无法进行三角化的匹配点,更新匹配关系Tracking.mvIniMatches
for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
{
if(mvIniMatches[i]>=0 && !vbTriangulated[i])
{
mvIniMatches[i]=-1;
nmatches--;
}
}
//5,将初始化的第一帧作为世界坐标系(变换为单位阵) ，当前帧(第二帧)的位姿 mCurrentFrame.mTcw 设为Rcw,tcw
mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
tcw.copyTo(Tcw.rowRange(0,3).col(3));
mCurrentFrame.SetPose(Tcw);
//6
// 功能：跟踪初始化时地图点的建立，及在相关模块上的必要更新
// 操作：
// 1,将成功初始化的第一帧和第二帧创建关键帧 KeyFrame
// 2,将两帧的描述子转化为BOW,结果存储在 KeyFrame.mBowVec 和 KeyFrame.mFeatVec 中
// 3,历两帧之间的匹配点,创建地图点 1关键帧中添加地图点 存储在 KeyFrame.mvpMapPoints中,
// 2 地图点中添加观测者(就是这两个关键帧) 存储在 MapPoint.mObservations,类型map<KeyFrame*,size_t>:前者存储关键帧索引，后者是该地图点在关键帧中索引
// 3,更新地图点“最好描述符” MapPoint.mDescriptor (Best descriptor to fast matching)
// 4,更新 MapPoint.mfMaxDistance,MapPoint.mfMinDistance,MapPoint.mNormalVector.
// 5,将地图点添加到地图中 Map.mspMapPoints
// 4,更新两个关键帧共视图(covisibility graph),其节点是关键帧,边权是关键帧之间共有的地图点个数
// 5,对地图进行BA优化
// 6,将MapPoints的中值深度归一化到1，并归一化两帧之间变换(单目传感器无法恢复真实的深度，这里将点云中值深度（欧式距离，不是指z）归一化到1)
// 更新:
// Frame 1,将创建的地图点存储在当前帧上, Frame.mvpMapPoints 存储地图点, Frame.mvbOutlier 更新标志位
// 2,当前帧位姿也设为归一化后的位姿
// 3,Frame.mpReferenceKF 当前帧的参考关键帧设为自己对应的关键帧
// KeyFrame 1,当前关键帧的位姿更新为归一化后的位姿(第一帧的位姿为单位阵,不需要归一化)
// Tracking 1,Tracking.mnLastKeyFrameId,上一关键帧id;Tracking.mpLastKeyFrame,上一个关键帧:设为当前帧
// 2,Tracking.mvpLocalKeyFrames 追踪线程中用来存储局部关键帧的vector,放入两个关键帧
// 3,Tracking.mvpLocalMapPoints 追踪线程中用来存储局部地图点的vector,放入创建的地图点
// 4,Tracking.mpReferenceKF 参考关键帧 设为当前关键帧
// 5,Tracking.mLastFrame 上一帧设为当前帧
// LocalMapping1,局部地图插入两个关键帧 LocalMapping.mlpLoopKeyFrameQueue 这是局部建图线程处理关键帧的队列
// Map 1,Map.mspKeyFrames,地图中存储关键帧的set,放入两个关键帧插入地图
// 2,Map.mspMapPoints,地图中存储地图点的set,将地图点添加到地图中
// 3,Map.mvpReferenceMapPoints,参考地图点,设为创建的地图点
// 4,Map.mvpKeyFrameOrigins 存储原点关键帧,设为第一帧关键帧.
// mState 更新为OK
CreateInitialMapMonocular();
}
}
}
代码引用的CreateInitialMapMonocular()也很重要，但是我再此不太想粘贴过多代码，以注释的方式说明了代码的操作。
需要注意的是：
1，CreateInitialMapMonocular()中，通过
mpLocalMapper->InsertKeyFrame(pKFcur)
mpLocalMapper->InsertKeyFrame(pKFini);
将成功初始化的连续两帧图像建立的关键帧送入了局部建图线程队列 LocalMapping.mlpLoopKeyFrameQueue 中，已经启动的局部建图线程将自动处理进入的关键帧。
2，初始化成功，mstate=OK
frame-to-frame跟踪
void Tracking::Track(){
//跟踪初始化
bool bOK;//追踪状态标志位
if(!mbOnlyTracking) //Tracking.mbOnlyTracking 追踪模式标志位,初始化为false,指正常VO模式,有地图更新;如果用户修改为true,系统将不建立地图,只进行追踪。
{
if(mState==OK)//如果初始化成功/追踪正常
{
// 功能:检查上一帧中的MapPoints是否被替换
// 1,遍历上一帧中的地图点 Frame.mvpMapPoints,
// 2,并判断地图点是否被替换(MapPoint.mpReplaced 有值)
// 3.如果有值,将地图点替换为 MapPoint.mpReplaced
// 说明:在局部建图和闭环线程对关键帧进行优化和处理时,会融合/替换世界点,但是只处理了关键帧的世界点,在此更新帧的世界点
CheckReplacedInLastFrame();
//采用三种跟踪方式，
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
// 功能:通过参考关键帧,追踪当前帧
// 步骤: 1,将当前帧描述子转化为BOW,结果存储在 KeyFrame.mBowVec 和 KeyFrame.mFeatVec 中
// 2,通过bow对参考关键帧和当前帧中的特征点进行快速匹配,获得当前帧的地图点 存储在 Frame.mvpMapPoints
// 3,3D-2D 最小化重投影误差优化当前帧位姿 Frame.Tcw,并更新地图点的离群标志位 Frame.mvbOutlier
// 4,剔除当前帧的离群地图点 更新 Frame.mvpMapPoints
// 返回:如果正确匹配数目大于10,返回1
bOK = TrackReferenceKeyFrame();
}
else
{
bOK = TrackWithMotionModel(); //根据恒速模型进行跟踪
if(!bOK)
bOK = TrackReferenceKeyFrame();//
}
}
else { bOK = Relocalization();}//跟踪丢失启动重定位
}
else{ ...}//不考虑 mbOnlyTracking=1 的情况
mCurrentFrame.mpReferenceKF = mpReferenceKF; //设定当前帧的参考关键帧帧为 跟踪线程的参考关键帧
//局部地图个跟踪
//关键选择
}
需要注意的是跟踪函数对当前帧变量的更新：
a,匹配获得地图点 Frame.mvpMapPoints
b,优化更新当前帧位姿 Frame.Tcw
c,其中优化过程中要判断匹配的地图点是否可靠，更新地图点的离群标志位 Frame.mvbOutlier，并在最后剔除当前帧的离群地图点 更新 Frame.mvpMapPoints
局部地图跟踪
该局部地图指的是跟踪线程中存储的 局部关键帧Tracking.mvpLocalKeyFrames 局部地图点 Tracking.mvpLocalMapPoints。
Tracking::Track()代码:
void Tracking::Track()
{
//跟踪初始化
//frame-to-frame跟踪
if(!mbOnlyTracking){
if(bOK)//如果frame-to-frame跟踪正常
bOK = TrackLocalMap();//局部地图跟踪
}
else{...}//不考虑 mbOnlyTracking=1 的情况
//位姿优化成功
if(bOK)
mState = OK;
else
mState=LOST;
//关键帧选择
其中，Tracking::TrackLocalMap()主要分为以下几步：
//1,通过当前帧的共视关系建立 跟踪线程的局部关键帧,局部地图点
//2,匹配 局部地图点 和 当前帧特征，获得匹配。
//3,优化当前帧位姿,并判断匹配地图点中的离群点
//4,通过内点数量判断跟踪成功还是失败
bool Tracking::TrackLocalMap()
{
//功能: 1,更新 Map.mvpReferenceMapPoints 为 Tracking.mvpLocalMapPoints (这应该是处理上一帧时获得的局部地图点)
// 2,重置 Tracking.mvpLocalKeyFrames 为 当前帧的共视关键帧+当前帧的共视关键帧的 子关键帧/父关键帧/共视关键帧
// Tracking.mvpLocalMapPoints 为 Tracking.mvpLocalKeyFrames 中的所有地图点
// Tracking.mpReferenceKF 为与当前帧共视程度最高的关键帧
// 3,更新 当前帧的参考帧Frame.mpReferenceKF 为与当前帧共视程度最高的关键帧
UpdateLocalMap();
// 功能: 1,匹配地图点Tracking.mvpLocalMapPoints与当前帧特征点,结果存储在当前帧 Frame.mvpMapPoints 中
// 2,并更新当前帧地图点、Tracking.mvpLocalMapPoints的计数,使得当前帧中的地图点,或跟踪线程中的落入当前帧视野的地图点 MapPoint.mnVisible++
SearchLocalPoints();
// 功能:3D-2D 最小化重投影误差优化当前帧位姿 Frame.Tcw
// 更新:位姿 Frame.Tcw,离群点标志 Frame.mvbOutlier (若第i个关键点离群，将其标志位 Frame.mvbOutlier[i]置为 1)
// 返回:内点数量
Optimizer::PoseOptimization(&mCurrentFrame);
mnMatchesInliers = 0;
//根据匹配关系更新内点数量 Tracking.mnMatchesInliers 更新地图点 MapPoint.mnFound
for(int i=0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
if(!mCurrentFrame.mvbOutlier[i])
{
//
mCurrentFrame.mvpMapPoints[i]->IncreaseFound(); //当前帧世界点 MapPoint.mnFound++
if(!mbOnlyTracking)
{
if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
mnMatchesInliers++;// 记录当前帧跟踪到的MapPoints，用于统计跟踪效果
}
else
mnMatchesInliers++;
}
else if(mSensor==System::STEREO)
mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
}
}
//失败——如果近期发生过重定位,且内点数量少于50
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)
return false;
//失败——内点数量少于30
if(mnMatchesInliers<30)
return false;
else
return true;
}
需要注意的是，跟踪线程利用的时共视关系/共视图(covisibility graph)创建的局部关键帧和局部地图点，该方法之后还会反复用到。
关键帧选择
Tracking::Track()中相关代码：
void Tracking::Track(){
//跟踪初始化
//frame-to-frame跟踪
//局部地图跟踪
if(bOK)// 如果跟踪良好 考虑插入关键帧
{
//更新运动模型
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;
}
else
mVelocity = cv::Mat();
// 清除临时添加的 MapPoint ,该临时添加只在运动模型追踪时使用,暂不考虑
for(int i=0; i<mCurrentFrame.N; i++)
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(pMP)
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
mlpTemporalPoints.clear();
//检测并插入关键帧，
if(NeedNewKeyFrame())
CreateNewKeyFrame();
// 删除那些在bundle adjustment中检测为outlier的3D map点（在局部地图匹配后，单目情况没有按照mCurrentFrame.mvbOutlier及时删去当前帧中匹配点，而放到了这里。为什么？ ）
for(int i=0; i<mCurrentFrame.N;i++)
{
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// 跟踪失败,并且relocation也失败,重新Reset
if(mState==LOST)
{
if(mpMap->KeyFramesInMap()<=5)
{
cout << "Track lost soon after initialisation, reseting..." << endl;
mpSystem->Reset();
return;
}
}
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
mLastFrame = Frame(mCurrentFrame);
//记录位姿信息，用于轨迹复现.
if(!mCurrentFrame.mTcw.empty())
{
cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
mlRelativeFramePoses.push_back(Tcr);
mlpReferences.push_back(mpReferenceKF);
mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
mlbLost.push_back(mState==LOST);
}
else
{
mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
mlpReferences.push_back(mlpReferences.back());
mlFrameTimes.push_back(mlFrameTimes.back());
mlbLost.push_back(mState==LOST);
}
}
在frame-to-frame跟踪中，只考虑对参考帧的跟踪（不考虑使用运动模型的跟踪）
Tracking::Track()的最后一部分 其实就2句：
if(NeedNewKeyFrame())
CreateNewKeyFrame();
给出其注释：
// 判断当前帧是否为 关键帧
// 考虑因素：
// 1,标志位 a,局部地图被闭环检测使用,则不插入关键帧 使用的标志位 LocalMapping.mbStopped\LocalMapping.mbStopRequested
// b,查询局部地图管理器是否繁忙 标志位 LocalMapping.mbAcceptKeyFrames
// 2,插入时间: a,距离上次重定位;b,距离上次插入关键帧
// 3,跟踪效果, 主要考虑,当前帧在跟踪线程中的内点数量 Tracking.mnMatchesInliers 与 参考关键帧帧中地图点数量 两者的比值
if(NeedNewKeyFrame())
//1.判断修改建图线程中的标志位 LocalMapping.mbStopped (true:正常创建;false,将其置为true,并放弃创建)
//2.采用当前帧创建新关键帧：将Frame的相关参数传递给 建立的KeyFrame,并更新KeyFrame的相关位姿，同时KeyFrame继承Frame的地图点 KeyFrame.mvpMapPoints = Frame.mvpMapPoints
//3.修改跟踪线程的相关参数
// a,参考帧tracking.mpReferenceKF 指向新建立的关键帧
// b,上一个关键帧id tracking.mnLastKeyFrameId 设为 Frame.mnId/KeyFrame.mnFrameId
// c,上一个关键帧 tracking.mpLastKeyFrame 指向新建立的关键帧
//4. 修改当前帧参考帧 Frame.mpReferenceKF 指向新建立的关键帧
//5. 向mapping线程发送新建立的帧,并修改标志位 LocalMapping.mbNotStop=0
CreateNewKeyFrame();
图像在Tracking线程中——Frame
按照之前分析结构，假设跟踪正常，且已经做过初始化，一张图像在跟踪线程中经历：
1,进入track()之间采用图像函数建立帧Frame
Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
//获取了特征点 Frame.mvKeys/ Frame.mvKeysUn 并计算出特征描述符 Frame.mDescriptors
2,然后采用参考帧跟踪当前位姿
bOK = TrackReferenceKeyFrame();
// 将当前帧描述子转化为BOW,结果存储在 KeyFrame.mBowVec 和 KeyFrame.mFeatVec 中
// 与Tracking参考帧进行特征匹配获得地图点:Frame.mvpMapPoints
// 在此基础上进行重投影误差优化，获得位姿 Frame.Tcw,(在优化中,获得离群标志:Frame.mvbOutlier)
// 按照Frame.mvbOutlier 删除离群当前帧地图点
3,设置当前帧的参考帧
mCurrentFrame.mpReferenceKF = mpReferenceKF; //这样来看,如果跟踪正常,这一步用不着
4,在局部地图中跟踪
bOK = TrackLocalMap()
// 当前帧的参考帧Frame.mpReferenceKF 为与当前帧共视程度最高的关键帧
// 与Tracking局部地图进行匹配，获得地图点:Frame.mvpMapPoints
// 在Tracking局部地图上进行 3D-2D 最小化重投影误差优化当前帧位姿 Frame.Tcw (更新 离群点标志 Frame.mvbOutlier)
5,判断是否作为关键帧
if(NeedNewKeyFrame()) CreateNewKeyFrame();
// 使用当前帧创建当前关键帧
// 将当前关键帧送入 局部建图线程
// 当前帧参考帧 Frame.mpReferenceKF 指向新建立的关键帧
6,按照Frame.mvbOutlier 删除离群当前帧地图点
即，创建当前帧后，计算特征点，描述符，计算BOW特征，然后分别在针对tracking参考帧和tracking局部地图进行了两套 <地图点匹配—优化位姿—删除离群地图点>。最后，创建关键帧，送入局部建图线程。
LocalMapping线程
线程创建
线程创建较为简单，下面是系统初始化函数System::System()引用LocalMapping线程创建的部分 和 线程创建使用的函数。
System::System(){
//创建地图
mpMap = new Map();
//LocalMapping线程创建
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
//LocalMapping线程启动
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);//启动mapping线程
}
//LocalMapping线程创建,仅是参数的初始值设置
LocalMapping::LocalMapping(Map *pMap, const float bMonocular):
mbMonocular(bMonocular), mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true)
{
}
线程运行 LocalMapping::Run()
线程持续运行函数，LocalMapping::Run()，函数监听队列 LocalMapping.mlNewKeyFrames。跟踪线程中创建关键帧后，采用以下代码将关键帧送入队列中。
void Tracking::CreateNewKeyFrame()
{
...
//5. 向mapping线程发送新建立的帧
mpLocalMapper->InsertKeyFrame(pKF);
...
}
//LocalMapping中的接收函数
void LocalMapping::InsertKeyFrame(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutexNewKFs);
mlNewKeyFrames.push_back(pKF);
mbAbortBA=true;//一个标志位，如果线程繁忙，置1，希望LocalMapping停止BA
}
让我们看 LocalMapping::Run() 函数
void LocalMapping::Run()
{
mbFinished = false;
while(1)
{
//标志位 LocalMapping设为false,同时当处理完队列里面关键帧时,设为了true,这是LocalMapping繁忙的标志,是Tracking中关键帧判断的一个标志
SetAcceptKeyFrames(false);
//队列 中是否有关键帧
if(CheckNewKeyFrames())
{
//对当前关键帧做一些处理
// 1,计算关键帧BoW,结果存储在 KeyFrame.mBowVec,KeyFrame.mFeatVec,
// 2,遍历当前关键帧的地图点,更新地图点信息(因为关键帧创建时只继承了帧的地图点信息,地图点关于当前帧的信息没有更新,在此进行更新)
// MapPoint.mObservations 添加当前关键帧的观测
// 更新 MapPoint.mfMaxDistance,MapPoint.mfMinDistance,MapPoint.mNormalVector,以及 MapPoint.mDescriptor
// 3,更新当前帧的共视关系/共视图——主要指
// KeyFrame.mConnectedKeyFrameWeights 记录map<共视帧,共视值>,
// KeyFrame.mvOrderedWeights 记录由大到小的共视帧,
// KeyFrame.mvOrderedWeights 记录由大到小的共视值
// KeyFrame.mpParent KeyFrame.mspChildrens
// 4,Map.mspKeyFrames 添加当前关键帧
ProcessNewKeyFrame();
//将待检测点集LocalMapping.mlpRecentAddedMapPoints中
// 1,质量不好打的点删除(MapPoint.SetBadFlag()),并从点集中剔除
// a,已经标记为坏点,MapPoint.mbBad=1
// b,跟踪到该MapPoint的Frame数(MapPoint.mnFound)相比预计可观测到该MapPoint的Frame数(MapPoint.mnVisible)的比例需大于25%
// c,当前帧关键帧id,KeyFrame.mnId - 地图点建立时关键帧id,MapPoint.mnFirstKFid 超过一帧;地图点观测数MapPoint.nObs不超过2,
// 2,质量好的点,不删除,仅从点集中剔除
// a,当前帧关键帧id,KeyFrame.mnId - 地图点建立时关键帧id,MapPoint.mnFirstKFid 超过两帧,
// 3,尚不确定的点保留。
MapPointCulling();
// 功能：当前关键帧和共视程度比较高的关键帧通过三角化恢复出一些MapPoints
// 步骤：取出当前帧关键帧 的 共视程度最高的20张共视关键帧,循环每张共视关键帧
// 1,计算共视关键帧的场景深度中值作为景深,和,当前关键帧和共视关键帧的基线长(即,两个关键帧之间的位移),两者之比如果过小,即两帧相距比较远,放弃该共视关键帧的后续处理
// 2,通过极线约束限制匹配时的搜索范围，共视关键帧和当前关键帧进行特征点匹配
// 3,对每对匹配通过三角化生成3D点:
// a,将特征点转移到世界坐标系中,获得两帧对该点的视角余弦值,三角化恢复3D点,并检测生成的3D点是否在相机前方
// b,将3D点分别投影到当前关键帧帧和共视关键帧中,计算3D点在当前关键帧下的重投影误差,满足阈值要求的3D点继续
// c,三角化生成3D点成功，构造成MapPoint
// 创建地图点, MapPoint.mObservations 添加当前关键帧和共视关键帧
// 并更新 MapPoint.mDescriptor,MapPoint.mfMaxDistance,MapPoint.mfMinDistance,MapPoint.mNormalVector.
// 当前关键帧和共视关键帧 KeyFrame.mvpMapPoints 添加该地图点
// 地图 Map.mspMapPoints,添加地图点
// 局部地图待检测点集 LocalMapping.mlpRecentAddedMapPoints 添加该世界点
CreateNewMapPoints();
if(!CheckNewKeyFrames()) //如果队列中没有待处理的关键帧了
{
// 融合当前关键帧和当前关键帧的两级共视关键帧(两级共视,即 当前帧的共视关键帧+当前关键帧的共视关键帧的共视关键帧)的相同地图点，并更新相关参数。
// 1取出当前关键帧的两级共视关键帧,遍历每个共视关键帧:
// 投影当前帧的MapPoints 到相邻关键帧中，并判断是否有重复的MapPoints:
// 如果MapPoint能匹配共视关键帧的特征点，并且该点有对应的MapPoint，那么将两个MapPoint合并：
// MapPoint/观测者少的地图点->Replace(MapPoint/观测者多的点)
// 如果MapPoint能匹配共视关键帧的特征点，但是该点没有对应的MapPoint，那么为该特征点点添加 MapPoint：
// MapPoint->AddObservation(pKFi,bestIdx)——该地图点 MapPoint.mObservations 添加来自共视关键帧的观测
// KeyFrame->AddMapPoint(pMP,bestIdx)——共视关键帧 KeyFrame.mvpMapPoints 添加该地图点
// 2取出所有二级共视关键帧的地图点，
// 与上一步相同, 投影 共视帧的地图点 到当前关键帧上， 融合重复的地图点
// 3更新当前关键帧地图点 MapPoint.mDescriptor,MapPoint.mfMaxDistance,MapPoint.mfMinDistance,MapPoint.mNormalVector
// 4更新当前帧的共视关系
// KeyFrame.mConnectedKeyFrameWeights
// KeyFrame.mvOrderedWeights 记录由大到小的共视帧, KeyFrame.mvOrderedWeights 记录由大到小的共视值
// KeyFrame.mpParent KeyFrame.mspChildrens
SearchInNeighbors();
}
mbAbortBA = false; //准备进行BA,标志位
if(!CheckNewKeyFrames() && !stopRequested())
{
// Local BA
if(mpMap->KeyFramesInMap()>2)
//BA优化：当前关键帧和共视关键帧的位姿、和它们所有的地图点的位置
//即更新了：KeyFrame.Tcw 等位姿参数;MapPoint.Pos,并MapPoint.UpdateNormalAndDepth();
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
// 功能：关键帧剔除,在Covisibility Graph中的关键帧,其90%以上的MapPoints能被其他关键帧(至少3个)观测到,则认为该关键帧为冗余关键帧。
// 步骤：遍历当前关键帧的共视关键帧,如果 90%以上的MapPoints能被其他关键帧(至少3个)观测到,KeyFrame.SetBadFlag()(从共视关系中、树结构中、地图点中等所有方面删除)
KeyFrameCulling();
}
//向闭环线程队列中送入当前关键帧
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
else if(Stop()) //如果 mbStopRequested =1 且 mbNotStop=0 将 mbStopped=1 并返回true
{
while(isStopped() && !CheckFinish()) //mbStopped=1 且 mbFinishRequested=0 将继续停止循环
{
usleep(3000);
}
if(CheckFinish()) //检查 mbFinishRequested 停止位为1,线程停止
break;
}
// 如果 mbResetRequested=1: 清空队列 LocalMapping.mlNewKeyFrames 清空待检测点集 LocalMapping.mlpRecentAddedMapPoints 标志位 LocalMapping.mbResetRequested 置零
ResetIfRequested();
SetAcceptKeyFrames(true); //完成当前帧处理 ,mbAcceptKeyFrames置为1 可以接受新帧
if(CheckFinish()) //如果有停止请求(mbFinishRequested=1),停下线程
break;
usleep(3000);
}
SetFinish(); //线程被请求停止,将标志位 mbFinished = true、mbStopped = true;
}
其实可以看出，LocalMapping线程所使用的“局部地图”，也是采用共视关系连接的关键帧和地图点集，并采用BA优化优化关键帧位姿和地图点位置。
图像在LocalMapping中——KeyFrame
在LocalMapping中，算法的处理重点已经不单独在关键帧上了，而是以共视关系为纽带的局部地图上，包括对“局部地图”中地图点的创建与融合，对“局部地图”中的关键帧和地图点的优化，对“局部地图”中不良地图点和关键帧的剔除。
//tracking 末尾：
KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
//创建当前关键帧,从Frame中继承:特征点数量N,特征点mvKeys,mvKeysUn,描述符mDescriptors,BoW mBowVec,mFeatVec,地图点mvpMapPoints,位姿mTcw
mpLocalMapper->InsertKeyFrame(pKF); //向mapping线程发送 关键帧
//进入LocalMapping
ProcessNewKeyFrame();
//计算关键帧BoW,结果存储在 KeyFrame.mBowVec,KeyFrame.mFeatVec(实际上已经从Frame继承,且和Frame中计算方法一致)
// 遍历当前关键帧的地图点,更新地图点信息(因为关键帧创建时只继承了帧的地图点信息,地图点关于当前帧的信息没有更新,在此进行更新)
// 更新MapPoint.mObservations,MapPoint.mfMaxDistance,MapPoint.mfMinDistance,MapPoint.mNormalVector,以及 MapPoint.mDescriptor
//更新当前帧的共视关系
// 更新 KeyFrame.mConnectedKeyFrameWeights 记录map<共视帧,共视值>, KeyFrame.mvOrderedWeights,KeyFrame.mvOrderedWeights,KeyFrame.mpParent,KeyFrame.mspChildrens
CreateNewMapPoints();
//当前关键帧和共视程度比较高的关键帧通过三角化恢复出一些MapPoints,建立连接关系，并更新地图点信息
SearchInNeighbors();
//融合当前关键帧和当前关键帧的两级共视关键帧的相同地图点，并更新相关参数。
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
//BA优化：当前关键帧和共视关键帧的位姿、和它们所有的地图点的位置
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
向闭环线程队列中送入当前关键帧
//与此同时,LocalMapping使用两个函数,分别剔除效果不好的地图点,和重复度高的关键帧。
KeyFrameCulling();//过滤关键帧
MapPointCulling();//过滤地图点
扫一扫
2258
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
