第二章 前端与重定位
前端
ORB整体软件框架
前言
进程和线程的概念：
进程(Process)是操作系统分配资源的基本单位，一个进程拥有的资源有自己的堆、栈、虚存空间(页表)、文件描述符等信息。线程无地址空间，它包括在进程的地址空间里。一个进程里面可以开多个线程共用一些资源
线程之间的通讯：
共享内存、消息队列、套接字（互斥锁、读写锁、信号量、条件变量）
ORB的数据结构
-
ORBSLAM进程开了三个线程：
追踪、局部建图、闭环检测
-
主要的逻辑文件：
System.cpp class system
initializer.cpp class Initializer
Tracking.cpp class Tracking
LocalMapping.cpp class LocalMapping
LoopClosing.cpp class LocalClosing
Viewer.cpp class Viewer -
主要的类：
Class Frame
Class KeyFrame
class KeyFrameDatabase
class MapPoints
class Map
Class Optimizer -
主要的数据结构：
Frame、KeyFrame、MAP、PLACE_RECOGNITION;
VO的流程
初始化、局部BA（追踪、建图、追踪[重定位]）、建图
初始化开始之后，肯定要tracking，tracking的步骤是追踪已经建好的3D点，作用是恢复现在的位姿，在恢复现在位姿以后，要进行建图，建立新的3D点为后面tracking服务，就这样tracking、Mapping、tracking一步一步的恢复自己的位姿、在追踪建图和追踪之间插入BA来优化地图点和位姿，也就是说BA开始之前需要有一个比较好的初始值，来进行SLAM中的滤波处理。
Tracking线程
出入Frame
初始化：回复位姿和Rt
相机位姿跟踪：位姿跟踪[通过恒速运动模型跟踪和通过关键帧跟踪]和重定位、只定位（在已经建立好的地图上），连续两帧的位姿用恒速运动模型。如果连续两帧算的跪了，用参考关键帧的初值再试一试。
局部地图跟踪：更新局部地图，恢复局部地图与当前帧的匹配、最小化投影误差优化位姿
判断是否生成关键帧：时间太长、线程空闲、跟踪要跪、跟踪的局部地图点太少
输出关键帧
-
初始化
初始化的时候 先算F矩阵和H矩阵打分，选分数高的模型
-
相机跟踪
TrackingWithMotionModel根据恒速运动模型设定当前帧的位姿 将上一帧与当前帧匹配 SearchByProjection 如果匹配太少，加大匹配范围阈值重新匹配 通过g2o优化位姿 剔除 Outlier MapPointsTrackWithReferenceKeyframe
当前帧计算bag of words 将上一帧与当前帧匹配 参考帧位姿设定为当前帧位姿 通过g2o优化位姿 提出Outlier MapPoints -
重定位
当前帧计算bag of words
筛选匹配候选关键帧
将候选帧依次与当前帧匹配（利用bow加速匹配）
选择匹配特征数大于阈值（15）的关键帧
EPnP求解位姿，g2o位姿优化
若inlier较少，在当前帧与候选帧之间进一步匹配（利用候选帧MapPoint投影）
-
筛选匹配候选关键帧
找到与当前帧有相同word的所有关键帧 BowVector（词和权重）
统计与当前帧有相同words最多的单词数->maxCommonWords
设定候选帧阈值=maxCommonWords*0.8
筛选出大于阈值的候选帧
根据关键帧连接关系分组，计算组得分，记录组中最高得分关键帧，以及所有组中得分最高的得分，实际上是空间一致性（按理说 当前帧应该和关键帧附近空间内的关键帧都有一定的相似程度 这才是好的匹配）
在组得分最高的候选帧中选择大于阈值的关键帧
-
void Tracking::Track()
{
// track包含两部分：估计运动、跟踪局部地图
// mState为tracking的状态机
// SYSTME_NOT_READY, NO_IMAGE_YET, NOT_INITIALIZED, OK, LOST
// 如果图像复位过、或者第一次运行，则为NO_IMAGE_YET状态
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
// mLastProcessedState存储了Tracking最新的状态，用于FrameDrawer中的绘制
mLastProcessedState=mState;
// Get Map Mutex -> Map cannot be changed
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// 步骤1：初始化
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
MonocularInitialization();
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
else// 步骤2：跟踪
{
// System is initialized. Track Frame.
// bOK为临时变量，用于表示每个函数是否执行成功
bool bOK;
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
// 在viewer中有个开关menuLocalizationMode，有它控制是否ActivateLocalizationMode，并最终管控mbOnlyTracking
// mbOnlyTracking等于false表示正常VO模式（有地图更新），mbOnlyTracking等于true表示用户手动选择定位模式
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
// 检查并更新上一帧被替换的MapPoints
// 更新Fuse函数和SearchAndFuse函数替换的MapPoints
CheckReplacedInLastFrame();
// 步骤2.1：跟踪上一帧或者参考帧或者重定位
// 运动模型是空的或刚完成重定位
// mCurrentFrame.mnId<mnLastRelocFrameId+2这个判断不应该有
// 应该只要mVelocity不为空，就优先选择TrackWithMotionModel
// mnLastRelocFrameId上一次重定位的那一帧
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
// 将上一帧的位姿作为当前帧的初始位姿
// 通过BoW的方式在参考帧中找当前帧特征点的匹配点
// 优化每个特征点都对应3D点重投影误差即可得到位姿
bOK = TrackReferenceKeyFrame();
}
else
{
// 根据恒速模型设定当前帧的初始位姿
// 通过投影的方式在参考帧中找当前帧特征点的匹配点
// 优化每个特征点所对应3D点的投影误差即可得到位姿
bOK = TrackWithMotionModel();
if(!bOK)
// TrackReferenceKeyFrame是跟踪参考帧，不能根据固定运动速度模型预测当前帧的位姿态，通过bow加速匹配（SearchByBow）
// 最后通过优化得到优化后的位姿
bOK = TrackReferenceKeyFrame();
}
}
else
{
// BOW搜索，PnP求解位姿
bOK = Relocalization();
}
}
else
{
// Localization Mode: Local Mapping is deactivated
// 只进行跟踪tracking，局部地图不工作
// 步骤2.1：跟踪上一帧或者参考帧或者重定位
// tracking跟丢了
if(mState==LOST)
{
bOK = Relocalization();
}
else
{
// mbVO是mbOnlyTracking为true时的才有的一个变量
// mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常，
// mbVO为true表明此帧匹配了很少的MapPoints，少于10个，要跪的节奏
if(!mbVO)
{
// In last frame we tracked enough MapPoints in the map
// mbVO为0则表明此帧匹配了很多的3D map点，非常好
if(!mVelocity.empty())
{
bOK = TrackWithMotionModel();
// 这个地方是不是应该加上：
// if(!bOK)
// bOK = TrackReferenceKeyFrame();
}
else
{
bOK = TrackReferenceKeyFrame();
}
}
else
{
// 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.
// mbVO为1，则表明此帧匹配了很少的3D map点，少于10个，要跪的节奏，既做跟踪又做定位
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)
{
// 这段代码是不是有点多余？应该放到TrackLocalMap函数中统一做
// 更新当前帧的MapPoints被观测程度
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;
}
}
}
// 将最新的关键帧作为reference frame
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// If we have an initial estimation of the camera pose and matching. Track the local map.
// 步骤2.2：在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
// local map:当前帧、当前帧的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();
}
if(bOK)
mState = OK;
else
mState=LOST;
// Update drawer
mpFrameDrawer->Update(this);
// If tracking were good, check if we insert a keyframe
if(bOK)
{
// Update motion model
if(!mLastFrame.mTcw.empty())
{
// 步骤2.3：更新恒速运动模型TrackWithMotionModel中的mVelocity
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; // Tcl
}
else
mVelocity = cv::Mat();
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
// Clean VO matches
// 步骤2.4：清除UpdateLastFrame中为当前帧临时添加的MapPoints
for(int i=0; i<mCurrentFrame.N; i++)
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(pMP)
// 排除UpdateLastFrame函数中为了跟踪增加的MapPoints
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Delete temporal MapPoints
// 步骤2.5：清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）
// 步骤2.4中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
// 这里生成的仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
// 这里不仅仅是清除mlpTemporalPoints，通过delete pMP还删除了指针指向的MapPoint
mlpTemporalPoints.clear();
// Check if we need to insert a new keyframe
// 步骤2.6：检测并插入关键帧，对于双目会产生新的MapPoints
if(NeedNewKeyFrame())
CreateNewKeyFrame();
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
// if they are outliers or not. We don't want next frame to estimate its position
// with those points so we discard them in the frame.
// 删除那些在bundle adjustment中检测为outlier的3D map点
for(int i=0; i<mCurrentFrame.N;i++)
{
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Reset if the camera get lost soon after initialization
// 跟踪失败，并且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);
}
// Store frame pose information to retrieve the complete camera trajectory afterwards.
// 步骤3：记录位姿信息，用于轨迹复现
if(!mCurrentFrame.mTcw.empty())
{
// 计算相对姿态T_currentFrame_referenceKeyFrame
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
{
// This can happen if tracking is lost
// 如果跟踪失败，则相对位姿使用上一次值
mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
mlpReferences.push_back(mlpReferences.back());
mlFrameTimes.push_back(mlFrameTimes.back());
mlbLost.push_back(mState==LOST);
}
}
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
